interrupt VectorNumber_Vsci0 void SCI0_ISR(void)
{
char sci_data;static i=0;
char data_tem;
int j=0;
if(SCI0SR1_RDRF)
{
sci_data = SCI0DRL;
if(sci_data=='S')
{
Motor_brake();
}
else if(sci_data=='G')
{
Motor_forward(15);
}
else if(sci_data!='\n')
{
str_rec[i]=sci_data;
i++;
}
else
{
i=0;
if(str_rec[i]=='s'||str_rec[i]=='g')
{
data_tem=str_rec[i];
for(j=1;j<strlen(str_rec);j++)
{
str_rec[j-1]=str_rec[j];
}
j=0;
if(data_tem=='s')
{
steer_control_P=atof(str_rec);
Sci0_send_strings("the steer_control_P is");
send_pc(steer_control_P);
}
else if(data_tem=='g')
{
speed=atof(str_rec);
Motor_forward(speed);
Sci0_send_strings("the speed is");
send_pc(speed);
}
}
}
}
}
int main(int argc, char **argv) {
Motor_init();
Sensor_init();
Button_init();
Compass_init();
turnCount = 0;
while(1) {
int front = (Sensor_getDistance(Sensor_FRONT_LEFT) +
Sensor_getDistance(Sensor_FRONT_RIGHT)) / 2;
while(front > 20) {
if(Compass_inRange(startDir, 15, 180))
Motor_steerLeft(0.9);
else if(Compass_inRange(startDir, 180, 45))
Motor_steerRight(0.9);
else
Motor_forward();
}
hugObstacle();
}
Motor_cleanUp();
Sensor_cleanUp();
Button_cleanUp();
}
void line_tracking_OUT_GRID(u08 speed_den)
{
u08 L_speed, R_speed, VR_DETERMINED_speed;
s08 fro_patt = 0;
ADC_update();
delay_ms(20);
VR_DETERMINED_speed = VR[0] / speed_den;
while(1)
{
fro_patt = patt_ana(front_sensor);
if(fro_patt > 4)
{
fro_patt = 0;
}
L_speed = LEFT_SPEED_BASE + VR_DETERMINED_speed + fro_patt;
R_speed = RIGHT_SPEED_BASE + VR_DETERMINED_speed - fro_patt;
Motor_forward(L_speed, R_speed);
delay_us(100);
if((middle_sensor & 0b00101100) != 0)
{
Motor_stop();
return;
}
}
}
void no_line_backward(u08 speed_den)
{
u08 L_speed, R_speed, VR_DETERMINED_speed;
ADC_update(); //check the VR value
delay_ms(20);
VR_DETERMINED_speed = VR[0] / speed_den;
L_speed = BACK_LEFT_SPEED_BASE + VR_DETERMINED_speed;
R_speed = BACK_RIGHT_SPEED_BASE + VR_DETERMINED_speed;
Motor_backward(L_speed, R_speed);
while((middle_sensor & 0b00000011) == 0);
Motor_stop();
Motor_forward(VR[0] / ADJUST_SPEED_DENOMINATOR , VR[0] / ADJUST_SPEED_DENOMINATOR);
while((middle_sensor & 0b00000011) == 0b00000000)
{
;
}
while((middle_sensor & 0b00000011) != 0b00000000)
{
;
}
Motor_stop();
}
void line_tracking_INTO_GRID(u08 speed_den)
{
u08 L_speed, R_speed, tracking_speed;
s08 fro_patt = 0;
ADC_update();
delay_ms(20);
tracking_speed = VR[0] / speed_den;
while(1)
{
fro_patt = patt_ana(front_sensor);
L_speed = tracking_speed + fro_patt;
R_speed = tracking_speed - fro_patt;
Motor_forward(R_speed, L_speed);
delay_us(100);
if(fro_patt > 4)
{
Motor_stop();
break;
}
}
Motor_forward(tracking_speed, tracking_speed);
while((front_sensor & 0b00111100) != 0);
Motor_stop();
// Motor_backward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR);
// while((middle_sensor & 0b00000011) == 0b00000000)
{
;
}
// while((middle_sensor & 0b00000011) != 0b00000000)
{
;
}
Motor_stop();
}
void Golfer_forward_p(u08 t)
{
u08 L_speed, R_speed, VR_DETERMINED_speed;
ADC_update(); //check the VR value
delay_ms(20);
VR_DETERMINED_speed = VR[0]/SPEED_DENOMINATOR_LOW;
L_speed = BACK_LEFT_SPEED_BASE + VR_DETERMINED_speed;
R_speed = BACK_RIGHT_SPEED_BASE + VR_DETERMINED_speed;
Motor_forward(L_speed, R_speed);
delay_ms(t);
Motor_stop();
}
void car_speed(void) //增量式PID正常处理部分
{
int pwmtemp;
//set_speed=25;
error=set_speed-Current_speed;
pwmtemp=PWMDTY01+PID_P*(error-last_error)+PID_I*(error)+PID_D*(error+pre_error-2*last_error);
if(pwmtemp<25)
{
pwmtemp = 25;
}
else if(pwmtemp>35)
{
pwmtemp = 35;
}
Motor_forward(pwmtemp);
pre_error=last_error;
last_error=error;
}
void no_line_forward(u08 speed_den)
{
u08 L_speed, R_speed, VR_DETERMINED_speed, local_counter = 0;
ADC_update(); //check the VR value
delay_ms(20);
VR_DETERMINED_speed = VR[0] / speed_den;
L_speed = LEFT_SPEED_BASE + VR_DETERMINED_speed - 1;
R_speed = RIGHT_SPEED_BASE + VR_DETERMINED_speed + 1;
Motor_forward(L_speed, R_speed);
while((middle_sensor & 0b00101100) == 0 && local_counter < 100)
{
local_counter++;
delay_ms(5);
}
Motor_stop();
}
void main(void)
{
char send_cnt=0;
SetBusCLK_40M();
SCI0_Init();
PIT_Init();
AD_Init();
CCD_IO_Init();
PWM_Init();
PAC_Init();
DDRT_DDRT0=1;
PTT_PTT0=1;
delay();
Motor_forward(26);
steering(STEER_MID);
DDRM=0XFF;
EnableInterrupts;
for(;;)
{
if(TimerFlag20ms == 1)
{
DisableInterrupts;
TimerFlag20ms = 0;
ImageCapture(Pixel);
//CalculateIntegrationTime();
//mid_val_3(Pixel);
//send_cnt++;
/*if(send_cnt>10)
{
send_cnt=0;
SendImageData(Pixel);
} */
find(Pixel,5,3,20);
//CCD_P2(Pixel,3,18);
steer_pd();
EnableInterrupts;
}
}
}
void line_backward(u08 mode_no, u08 speed_den, u08 step_local)
{
u08 L_speed, R_speed, VR_DETERMINED_speed;
s08 fro_patt, back_patt, drift, defl;
s16 diff = 0;
u08 STEP = 0, STEP_flag = 0, flag_local = 0;
if(step_local == 0)
{
return;
}
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
past_defl = 0;
past_front = 0;
past_back = 0;
VR_DETERMINED_speed = VR[0] / speed_den;
while (1)
{
L_speed = BACK_LEFT_SPEED_BASE + VR_DETERMINED_speed;
R_speed = BACK_RIGHT_SPEED_BASE + VR_DETERMINED_speed;
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
past_front = fro_patt;
}
else //front sensor invalid
{
fro_patt = past_front;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
past_back = back_patt;
}
else //front sensor invalid
{
back_patt = past_back;
}
drift = fro_patt + back_patt;
defl = back_patt - fro_patt;
diff = VR_DETERMINED_speed * (- 15 * drift - 35 * defl - 380 / VR_DETERMINED_speed * (defl - past_defl));
L_speed = BACK_LEFT_SPEED_BASE + VR_DETERMINED_speed - diff / 210;
R_speed = BACK_RIGHT_SPEED_BASE + VR_DETERMINED_speed;
if(L_speed > 100)
L_speed = 100;
Motor_backward(L_speed, R_speed);
past_defl = defl;
delay_us(100); // make a little delay
if(((back_sensor & 0b10010000) == 0b10010000 || (back_sensor & 0b00001001) == 0b00001001) && STEP_flag == 0)
{
delay_ms(5);
if((back_sensor & 0b10010000) == 0b10010000 || (back_sensor & 0b00001001) == 0b00001001)
{
STEP_flag = 1;
}
}
if(((back_sensor & 0b10010000) != 0b10010000 && (back_sensor & 0b00001001) != 0b00001001) && STEP_flag == 1)
{
STEP_flag = 2;
}
if((middle_sensor & 0b00000011) != 0 && STEP_flag == 2)
{
delay_ms(5);
if((middle_sensor & 0b00000011) != 0)
{
STEP_flag = 0;
STEP++;
Map_setposition_back();
}
}
if (step_local == STEP)
{
STEP_flag = 3;
if(flag_local == 0 && (middle_sensor & 0b00000011) != 0b00000000)
{
Motor_stop();
Motor_forward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR);
while((middle_sensor & 0b00000011) == 0b0000000)
{
;
}
while((middle_sensor & 0b00000011) != 0b0000000)
{
;
}
Motor_backward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR);
while((middle_sensor & 0b00000011) == 0b0000000)
{
;
}
Motor_stop();
return;
}
}
}
Motor_stop();
}
u08 line_tracking(u08 mode_no, u08 speed_den, u08 step_local)
{
u08 L_speed, R_speed, VR_DETERMINED_speed;
s08 fro_patt, back_patt, drift, defl;
s16 diff = 0;
u08 STEP_flag = 0, flag_local = 0, low_speed_flag = 0;
u08 side_ball_detected = 0;
u08 STEP = 0;
if(step_local == 0)
{
return 0;
}
ADC_update();
delay_ms(20);
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
past_defl = 0;
past_front = 0;
past_back = 0;
VR_DETERMINED_speed = VR[0] / speed_den;
if (back_patt == NO_LINE && low_speed_flag == 0)
{
low_speed_flag = 1;
VR_DETERMINED_speed = VR[0] / SPEED_DENOMINATOR_VERY_LOW;
}
while (1)
{
L_speed = LEFT_SPEED_BASE + VR_DETERMINED_speed;
R_speed = RIGHT_SPEED_BASE + VR_DETERMINED_speed;
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
// if(fro_patt == NO_LINE)
{
// Motor_stop();
// break;
}
if (back_patt != NO_LINE && low_speed_flag == 1)
{
low_speed_flag = 0;
VR_DETERMINED_speed = VR[0] / speed_den;
}
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
past_front = fro_patt;
}
else //front sensor invalid
{
fro_patt = past_front;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
past_back = back_patt;
}
else //front sensor invalid
{
back_patt = past_back;
}
drift = fro_patt + back_patt; //for example, when fro = 1, back = -1 (left)
defl = fro_patt - back_patt; //drift = 0, defl = 2.`
//another: fro = 1, back = 0
//drift = 1, defl = 1
diff = VR_DETERMINED_speed * (- 15 * drift - 35 * defl - 550 / VR_DETERMINED_speed * (defl - past_defl));
L_speed = LEFT_SPEED_BASE + VR_DETERMINED_speed - diff / 230;
R_speed = RIGHT_SPEED_BASE + VR_DETERMINED_speed;
if(L_speed > 100)
L_speed = 100;
Motor_forward(L_speed, R_speed);
past_defl = defl;
delay_us(100); // make a little delay
fro_patt = patt_ana(front_sensor);
if((middle_sensor & 0b00101100) != 0b00000000)
{
Motor_stop();
return FRONT_OBJECT_STOP * 10 + STEP;
}
if(((front_sensor & 0b10010000) == 0b10010000 || (front_sensor & 0b00001001) == 0b00001001) && STEP_flag == 0) // cross encounter
{
delay_ms(5);
if ((front_sensor & 0b10010000) == 0b10010000 || (front_sensor & 0b00001001) == 0b00001001)
{
STEP_flag = 1;
}
}
if(((front_sensor & 0b10010000) != 0b10010000 && (front_sensor & 0b00001001) != 0b00001001) && STEP_flag == 1)
{
STEP_flag = 2;
}
if((middle_sensor & 0b00000011) != 0 && STEP_flag == 2)
{
delay_ms(5);
if((middle_sensor & 0b00000011) != 0)
{
STEP_flag = 0;
STEP++;
Map_setposition_front();
}
}
if(mode_no == OBJECT_MODE || mode_no == SIDE_OBJECT_MODE)
{
if((front_sensor & 0b10000001) != 0 && side_ball_detected == 0)
{
if(1 == Golfer_detection())
{
side_ball_detected = 1;
}
}
}
if(side_ball_detected == 1)
{
Map_setpoint();
side_ball_detected = 2;
if(mode_no == SIDE_OBJECT_MODE)
{
STEP = step_local;
Map_setposition_front();
}
}
////////////////////////////////////////////////
//step check
if (step_local == STEP)
{
STEP_flag = 3;
if(flag_local == 0 && (middle_sensor & 0b00000011) != 0b00000000)
{
delay_ms(10);
Motor_stop();
Motor_backward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR);
while((middle_sensor & 0b00000011) == 0b0000000)
{
;
}
Motor_stop();
if(side_ball_detected == 2)
{
return SIDE_OBJECT_STOP * 10 + STEP;
}
else
{
return STEP_STOP * 10 + STEP;
}
}
}
} // end of while loop
Motor_stop();
return 0;
}