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 Golfer_turn_right_corner()
{
u08 L_speed, R_speed;
ADC_update(); //check the VR value
delay_ms(20);
L_speed = TURN_RIGHT_LEFT_BASE;
R_speed = TURN_RIGHT_RIGHT_BASE;
Motor_TurnRight(L_speed, R_speed); //motor.c
while(1)
{
if((front_sensor & 0b00011000) == 0)
break;
}
while(1)
{
if(front_sensor == 0b00011100 || front_sensor == 0b00011000)
{
break;
}
}
Motor_stop();
// L_speed = TURN_LEFT_LEFT_BASE * 7 / 8;
// R_speed = TURN_LEFT_RIGHT_BASE * 7 / 8;
// Motor_TurnLeft(L_speed, R_speed);
// while(1)
// {
// if(front_sensor == 0b00111000 || front_sensor == 0b00011000)
// break;
// }
// Motor_stop();
}
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 Golfer_turn_left()
{
u08 L_speed, R_speed;
ADC_update(); //check the VR value
delay_ms(20);
L_speed = TURN_LEFT_LEFT_BASE - 2;
R_speed = TURN_LEFT_RIGHT_BASE + 2;
Motor_TurnLeft(L_speed, R_speed); //motor.c
while(1)
{
if((front_sensor & 0b00111100) == 0)
break;
}
while(1)
{
if(front_sensor == 0b00111000 || front_sensor == 0b00011000)
{
break;
}
}
Motor_stop();
defl_adjust();
}
/*-----------------------------------------------------------------------*/
void init_Stepmotor(void)
{
SIU.PCR[16].R = 0x0203; /* A相 PB0 */
SIU.PCR[17].R = 0x0203; /* B相 PB1 */
SIU.PCR[72].R = 0x0203; /* C相 PE8 */
SIU.PCR[73].R = 0x0203; /* D相 PE9 */
Motor_stop();
}
void line_backward_BACK_TO_GRID(u08 speed_den)
{
u08 L_speed, R_speed, VR_DETERMINED_speed;
s08 back_patt = 0;
ADC_update();
delay_ms(20);
VR_DETERMINED_speed = VR[0] / speed_den;
while(1)
{
back_patt = patt_ana(back_sensor);
L_speed = BACK_LEFT_SPEED_BASE + VR_DETERMINED_speed + back_patt;
R_speed = BACK_RIGHT_SPEED_BASE + VR_DETERMINED_speed - back_patt;
Motor_backward(L_speed, R_speed);
delay_us(100);
if(back_patt > 4)
{
Motor_stop();
break;
}
}
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(1)
{
if((back_sensor & 0b00111100) ==0 )
{
Motor_stop();
break;
}
}
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_turn_right_special()
{
u08 L_speed, R_speed;
ADC_update(); //check the VR value
delay_ms(20);
L_speed = TURN_RIGHT_LEFT_BASE ;
R_speed = TURN_RIGHT_RIGHT_BASE;
Motor_TurnRight(L_speed, R_speed + 2); //motor.c
while(1)
{
if(back_sensor==0b000111000 || back_sensor==0b00111000 || back_sensor==0b00011000)
break;
}
Motor_stop();
// Motor_forward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR);
// while((middle_sensor & 0b00000001) == 0b00000000)
{
;
}
// Motor_stop();
Motor_TurnRight(L_speed + 0, R_speed + 3);
while(1)
{
if(front_sensor==0b00000000 || front_sensor==0b00000001 || front_sensor==0b10000001)
break;
}
while(1)
{
if(front_sensor == 0b00011100 || front_sensor == 0b00011000)
{
break;
}
}
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 Golfer_backward_p(void)
{
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_backward(L_speed, R_speed); //motor.c
while((middle_sensor & 0b00101100) != 0b00000000)
{
;
}
Motor_stop();
return;
}
void Golfer_turn_right_inverse()
{
u08 L_speed, R_speed;
ADC_update(); //check the VR value
delay_ms(20);
L_speed = TURN_RIGHT_LEFT_BASE - 5;
R_speed = TURN_RIGHT_RIGHT_BASE + 5;
Motor_TurnRight(L_speed, R_speed); //motor.c
while(1)
{
if(back_sensor==0b00111000 || back_sensor==0b00011100 || back_sensor==0b00011000)
break;
}
Motor_stop();
}
void TEST_display()
{
TEST_mode();
if(MODE)
{
// printf("ldc0_val");
// printf("%d \t",(int)LDC_SPI0_val);
// printf("ldc1_val");
// printf("%d \t",(int)LDC_SPI1_val);
// printf("%d\r\n",(int)LDC_result);
LCD_P6x8Str(0,4,"ldc0_val:");
LCD_BL(55,4,(uint16)LDC_SPI0_val);
LCD_P6x8Str(0,6,"ldc1_val:");
LCD_BL(55,6,(uint16)LDC_SPI1_val);
LCD_BL(90,4,(uint16)adjustment[1]);
LCD_BL(90,6,(uint16)adjustment[2]);
LCD_BL(70,2,(uint16)(divisor*100));
}
else
{
FLOAT_SPI_Read_Buf(LDC1000_CMD_REVID,&orgVal[0],12,SPI0);//orgVal[]对应上面写入的值说明初始化正常
for(int i=0;i<3;i++)
{
printf("%c0",orgVal[i]);
LCD_BL(0+(i*10),8,(uint16)orgVal[i]);
}
RPMIN_tmp_SPI0 = orgVal[2];
systick_delay_ms(150);
FLOAT_SPI_Read_Buf(LDC1000_CMD_REVID,&orgVal[0],12,SPI1);//orgVal[]对应上面写入的值说明初始化正常
for(int i=0;i<3;i++)
{
printf("%c1",orgVal[i]);
LCD_BL(0+(i*10),9,(uint16)orgVal[i]);
Motor_stop();
}
systick_delay_ms(150);
}
}
int main(void)
{
IO_init();
Timer1_init();
ADC_init();
Motor_stop();
Servo_init();
LCD_init();
sei();
while(1)
{
inherent_function();
}
while(1);
return 0;
}
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 Golfer_turn_right_edge()
{
u08 L_speed, R_speed, tracking_speed;
ADC_update(); //check the VR value
delay_ms(20);
tracking_speed = VR[0]/SPEED_TURNING_DENOMINATOR;
L_speed = tracking_speed;
R_speed = tracking_speed;
Motor_TurnRight(L_speed, R_speed); //motor.c
while(1)
{
if(patt_ana(front_sensor) == NO_LINE)
break;
}
while(1)
{
if(back_sensor==0b00111000 || back_sensor==0b00011100 || back_sensor==0b00011000)
break;
}
Motor_stop();
}
void main(void)
{
int i1=0;
int count10S = 0;
Device_init();
disable_irq(PIT0_IRQn);
systick_delay_ms(50);
FLOAT_LDC_init(SPI1);
systick_delay_ms(50);
FLOAT_LDC_init(SPI0);
systick_delay_ms(1000);
uart_init(UART3,115200);
key_init(0);
key_init(1);
key_init(2);
//LDC_EVM_TEST();
//Speed_Ctl_Init(short point_value, double dKpp, double dKii, double dKdd);
while(1)
{
//value_collect();
LCD_P6x8Str(0,2,"Rmax Lmin");
printf("Rmax Rmin\r\n");
systick_delay_ms(1500);
i1=50;
while(i1--)
{
adjustment[2]=(float)filter(SPI1)/10;//右max
LCD_BL(55,2,(uint16)adjustment[2]);
printf("%d\r\n",adjustment[2]);
}
i1=50;
while(i1--)
{
adjustment[1]=(float)filter(SPI0)/10;//左min
LCD_BL(90,2,(uint16)adjustment[1]);
printf("%d\r\n",adjustment[1]);
}
LCD_BL(55,2,(uint16)adjustment[1]);
LCD_BL(90,2,(uint16)adjustment[2]);
LCD_P6x8Str(0,4,"Lmax Rmin");
printf("Lmax Rmin\r\n");
systick_delay_ms(1500);
i1=50;
while(i1--)
{
adjustment[3]=(float)filter(SPI1)/10;//右min
LCD_BL(55,4,(uint16)adjustment[3]);
printf("%d\r\n",adjustment[3]);
}
i1=50;
while(i1--)
{
adjustment[4]=(float)filter(SPI0)/10;//左max
LCD_BL(90,4,(uint16)adjustment[4]);
printf("%d\r\n",adjustment[4]);
}
LCD_BL(55,4,(uint16)adjustment[4]);
LCD_BL(90,4,(uint16)adjustment[3]);
if(adjustment[1]<=adjustment[3])
adjustment[5]=adjustment[1];//次小值
else
adjustment[5]=adjustment[3];//次小值
if(adjustment[2]>=adjustment[4])
adjustment[6]=adjustment[2];//次大值
else
adjustment[6]=adjustment[4];//次大值
divisor = (adjustment[2] - adjustment[3])/60; //48 //50
divisor2 = (adjustment[4] - adjustment[1])/59; //48 //45
zengfuzhi = (adjustment[2] - adjustment[3])/(adjustment[4] - adjustment[1]);
// for(int i=0;i<10;i++)
// {
// printf("%d\r\n",adjustment[i]);
// }
// systick_delay_ms(1000);
LCD_P6x8Str(0,6,"Mid value");
i1 = 50;
systick_delay_ms(1500);
while(i1--)
{
adjustment[7]=(float)filter(SPI0)/10;//左
adjustment[8]=(float)filter(SPI1)/10;//右
LCD_BL(55,6,(uint16)adjustment[7]);
LCD_BL(90,6,(uint16)adjustment[8]);
}
if(adjustment[7]>=adjustment[8])
{ flag=1;
adjustment[0]=adjustment[7]-adjustment[8];
}
else
{
flag=0;
adjustment[0]=adjustment[8]-adjustment[7];
}
LCD_Fill(0x00);
Out_side_collect();
LCD_Fill(0x00);
adjustment_change();
enable_irq(PIT0_IRQn);
while(1)
{
if(PIT_1sFlag == 1)
{
gpio_turn(PTD4);
PIT_1sFlag = 0;
count10S ++;
}
if(key_check(2) == 0)
{
LCD_Fill(0x00);
Motor_stop();
break;
}
else
{
if(PIT_5msFlag == 1)
{
LDC_get();
PIT_5msFlag = 0;
Steering_Change();
}
if(count10S > 10)
{
if(gpio_get(PTC4) == 0)
{
while(1)
{
Motor_stop();
Steering_Change();
}
}
}
if(PIT_10msFlag == 1)
{
Quad_count();
now = (float)guad_val;
Motor_PID(now);
PIT_10msFlag = 0;
}
if(PIT_20msFlag == 1)
{
PIT_20msFlag = 0;
}
if(PIT_50msFlag == 1)
{
//Result_collect();
PIT_50msFlag = 0;
}
//printf("guad_val:%d \r\n",(guad_val));
//TEST_display();
speed_control();
//key_choice();
uint16 i23 = gpio_get(PTC4);
//LCD_BL(50,2,(uint16)(i23));
}
}
}
}
int main(void){
unsigned char Sensor = 0; //Storing IR value.
unsigned char prevSensor = 0; //Previous IR value.
port_Init();
while(1){
prevSensor = Sensor; //Storing Previous IR value.
Sensor = (~PIND) & 0x0F; //Reading IR Sensor.
//Use D0 ~ D3
//Blackline = 1, Whiteline = 0.
/*
sensor value black line position
0001 1 Left
0011 3 Left
0110 6 Middle
1100 12(C) Right
1000 8 Right
*/
switch(Sensor){
case 0 : //On White line.
case 0x0F :
Motor_stop();
break;
case 0x01 : //Turn left
case 0x02 :
case 0x03 :
PORTG = 0x02;
if(prevSensor != Sensor){ //If the black line positions have chaged
Motor_stop();
send_data(get_data);
}
Motor_1(MotorB);
break;
case 0x06 : //Straight
PORTG = 0x03;
if(prevSensor != Sensor){
Motor_stop();
send_data(get_data);
}
Motor_1(MotorB);
Motor_2(MotorA);
break;
case 0x04 :
case 0x08 :
case 0x0C :
PORTG = 0x01;
if(prevSensor != Sensor){
Motor_stop();
send_data(get_data);
}
Motor_2(MotorA);
break;
default :
PORTG = 0x03;
if(prevSensor != Sensor){
Motor_stop();
send_data(get_data);
}
Motor_1(MotorB);
Motor_2(MotorA);
break;
}
}
}
int main(void)
{
init(); //! Call init
uint8_t Go_A=0; //! Drive Command memory for direction A
uint8_t Go_B=0;
uint8_t key; //! Char code from keyboard, currently used for remote ctrl
while(1)
{
UPRINT("\f"); //! Terminal form feeds
// error_median_check(); //! Call Median check
error_limits_check(); //! Call Limit check
error_nfault_check(); //! Call nFault check
if (error_reg != 0) error_modul (); //! Fault detection
//! Call "error_modul" if an error occurs
if(GET_REMOTE_SWITCH) //! If the remote switch is set (PA4) the move direction can be remote controlled by pressing a or b...
{ //! ... on the keyboard of the PC which is connected through the UART with the motor control board
UPRINT("Modus: Remote\r\n");
key = uart_getc_nowait(); //! defining "key" with a key-input through the uart
UPRINT("Key: "); //! print out a probably pressed key on the remote console
UPRINTN(key);
UPRINT("\r\n");
if(key == 'a') //! If key "a" was pressed during remote mode, set Go_a=1 and Go_B=0
{
Go_A = 1;
Go_B = 0;
}
else if(key == 'b') //! If key "b" was pressed during remote mode, set Go_a=0 and Go_B=1
{
Go_A = 0;
Go_B = 1;
}
if(GET_BUTTON_A_RMT && GET_BUTTON_B_RMT) {Go_A = 0; Go_B = 0;} //! if both direction buttons are pressed simultaneously, Go_A and GO_B are 0, the motor stands still
if(GET_BUTTON_A_RMT) {Go_A = 1; Go_B = 0;} //! if switch A is pressed, set Go_a=1 and Go_B=0 for driving the motor to A
if(GET_BUTTON_B_RMT) {Go_A = 0; Go_B = 1;}
}
else //! If the remote mode is deactivated, the direction buttons must be pressed direct at the front panel
{
UPRINT("Modus: Local\r\n\r\n");
if(GET_BUTTON_A && GET_BUTTON_B) {Go_A = 0; Go_B = 0;} //! if both direction buttons are pressed simultaneously, Go_A and GO_B are 0, the motor stands still
if(GET_BUTTON_A) {Go_A = 1; Go_B = 0;} //! if switch A is pressed, set Go_a=1 and Go_B=0 for driving the motor to A
if(GET_BUTTON_B) {Go_A = 0; Go_B = 1;} //! if switch B is pressed, set Go_a=0 and Go_B=1 for driving the motor to B
}
UPRINTN(lastLimit); //! give out the last pressed limit switch to the remote console
UPRINT(" Lastlimit\r\n");
if(GET_LIMIT_A) TURN_ON(PORT_LMT_A_LED, LMT_A_LED); //! check if limit switch A is pressed and show it by LED on the front panel
else TURN_OFF(PORT_LMT_A_LED, LMT_A_LED); //! if limit switch A is not pressed, light off the limit LED for limit switch A
if(GET_LIMIT_B) TURN_ON(PORT_LMT_B_LED, LMT_B_LED); //! check if limit switch B is pressed and show it by LED on the front panel
else TURN_OFF(PORT_LMT_B_LED, LMT_B_LED); //! if limit switch B is not pressed, light off the limit LED for limit switch B
// if(GET_MOTOR_STOP) //! if the motor stop button is pressed...
// {
// Go_A = 0; Go_B = 0; //! ... set Go_A=0 and Go_B=0, ...
// Motor_stop(); //! ... call the "motorstop()" routine...
// TURN_OFF(PORT_DIR_A_LED , DIR_A_LED ); //! ... and light off both direction LED's
// TURN_OFF(PORT_DIR_B_LED , DIR_B_LED );
// }
// else //! if the motor stop button is not press, than check different status of the A/B buttons and limit switches
// {
if (GET_LIMIT_A && !(GET_LIMIT_B)) //! Limit switch A is pressed, Limit switch B is not pressed //Endschalter A gedrueckt, Endschalter B nicht gedreuckt
{
DBPRINT("LA=1 LB=0\r\n"); //! print out to the remote console which limit switch is pressed
if (Go_B) //! if button B is pressed...
{
Motor_RE(); //! ... call "Motor_RE" routine for driving the motor to B
TURN_ON(PORT_DIR_B_LED , DIR_B_LED ); //! turn on the LED for driving to B
TURN_OFF(PORT_DIR_A_LED , DIR_A_LED ); //! turn off the LED for driving A, if the motor was driving to A but not reached it before the driving direction was switched
UPRINT("Fahre Richtung B\r\n"); //! print out the driving direction int the remote console
}
if (Go_A) //! if button A is pressed, call "Motor_stop", because the motor can't drive to A, if the limit switch A is pressed
{
Go_A = 0;
Motor_stop(); //! Call Motor stop
TURN_OFF(PORT_DIR_A_LED , DIR_A_LED ); //! light off both direction LED's
TURN_OFF(PORT_DIR_B_LED , DIR_B_LED );
UPRINT("\r\n Stop bei A\r\n"); //! print out to the remote console that he motor stops/stand still
}
}
if (GET_LIMIT_B && !(GET_LIMIT_A)) //! Limit switch B is pressed, Limit switch A is not pressed //Endschalter A gedrueckt, Endschalter B nicht gedreuckt
{
DBPRINT("LA=0 LB=1\r\n"); //! print out to the remote console which limit switch is pressed
if (Go_A) //! if button A is pressed...
{
Motor_FW(); //! ... call "Motor_FW" routine for driving the motor to B
TURN_ON(PORT_DIR_A_LED , DIR_A_LED ); //! turn on the LED for driving to A
TURN_OFF(PORT_DIR_B_LED , DIR_B_LED ); //! turn off the LED for driving B, if the motor was driving to A but not reached it before the driving direction was switched
UPRINT("Fahre Richtung A\r\n"); //! print out the driving direction int the remote console
}
if (Go_B) //! if button B is pressed, call "Motor_stop", because the motor can't drive to A, if the limit switch A is pressed
{
Go_B = 0;
Motor_stop();
TURN_OFF(PORT_DIR_A_LED , DIR_A_LED ); //! light off both direction LED's
TURN_OFF(PORT_DIR_B_LED , DIR_B_LED );
UPRINT("\r\n Stop bei B\r\n"); //! print out to the remote console that he motor stops/stand still
}
}
if (!(GET_LIMIT_A) && !(GET_LIMIT_B)) //! none of the limit switches is pressed
{
DBPRINT("LA=0 LB=0\r\n");
if (Go_A) //! if button A is pressed ...
{
Go_B = 0;
Motor_FW(); //! call motor forward function
TURN_ON(PORT_DIR_A_LED , DIR_A_LED ); //! turn on direction LED A
TURN_OFF(PORT_DIR_B_LED , DIR_B_LED ); //! turn off direction LED B
UPRINT("Fahre Richtung A\r\n");
}
else if (Go_B) //! if button B is pressed...
{
Motor_RE(); //! call motor reverse function
TURN_ON(PORT_DIR_B_LED , DIR_B_LED ); //! turn on direction LED B
TURN_OFF(PORT_DIR_A_LED , DIR_A_LED ); //! turn off direction LED A
UPRINT("Fahre Richtung B\r\n");
}
}
//}
/*
medIDrv = median(&pRbIDrv->mem[0]); //! sample median for driver current
medU24 = median(&pRbU24->mem[0]); //! sample median for supply voltage
medUFuse = median(&pRbUFuse->mem[0]); //! sample median for fuse voltage
*/
medIDrv = dbg_medIDrv;
medU24 = dbg_medU24;
medUFuse = dbg_medUFuse;
DBPRINT("\r\n\r\nMedian output:\r\n"); //! print out the different median values
DBPRINTN(medIDrv);
DBPRINT("\t mA medIDrv \r\n");
DBPRINTN(medU24);
DBPRINT("\t mV medU24 \r\n");
DBPRINTN(medUFuse);
DBPRINT("\t mV medUFuse \r\n");
DBPRINT("\r\n\r\nMotor:\r\n");
DBPRINT("Typ\tStatus\tMotorDirection\tSLEEP\tENABLE\tPHASE\tMODE\r\n");
MOTOR_TYPE();
DBPRINT("\t");
if ( (PORT_DRV & ~DRV_EN) && (PORT_DRV & DRV_MODE) ) DBPRINT("BRAKE"); //! set Motor BRAKE
else if (!(PORT_DRV & DRV_SLEEP)) DBPRINT("SLEEP"); //! set Motor SLEEP
else if ((PORT_DRV & DRV_SLEEP)) DBPRINT("ARMED"); //! set Motor ARMED
else DBPRINT("Unknown"); // ?
DBPRINT("\t");
MOTOR_DIR();
DBPRINT("\t");
if (PORT_DRV & DRV_SLEEP) DBPRINT ("1"); //! if driver_sleep bit is set, set motor sleep 1 // motor sleep
else DBPRINT ("0");
DBPRINT("\t");
if (PORT_DRV & DRV_EN) DBPRINT("1"); //! if driver_enabled bir is set, set motor enable 1 // motor enable
else DBPRINT("0");
DBPRINT("\t");
if (PORT_DRV & DRV_PHASE) DBPRINT("1"); //! if driver_phase is set, set motor phase 1 // motor phase
else DBPRINT("0");
DBPRINT("\t");
if (PORT_DRV & DRV_MODE) DBPRINT("1"); //! if driver_mode is set, set motor mode 1 // motor mode
else DBPRINT("0");
DBPRINT("\r\n");
wdt_reset();
_delay_ms(100);
}
}
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;
}
void defl_adjust()
{
u08 L_speed, R_speed, tracking_speed;
s08 fro_patt, back_patt, defl;
//first phase
tracking_speed = VR[0] / SPEED_TURNING_DENOMINATOR;
L_speed = tracking_speed;
R_speed = tracking_speed;
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
}
else //front sensor invalid
{
fro_patt = 0;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
}
else //front sensor invalid
{
back_patt = 0;
}
defl = fro_patt - back_patt;
if(defl < 0) // left deflection
{
Motor_TurnLeft(L_speed, R_speed);
}
if(defl > 0) //right deflection
{
Motor_TurnRight(L_speed, R_speed);
}
while(1)
{
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
defl = fro_patt - back_patt;
if (defl == 0)
break;
}
Motor_stop();
/////////////////////////////////////////////////////////////
///second phase
tracking_speed = VR[0] / ADJUST_SPEED_DENOMINATOR;
L_speed = tracking_speed;
R_speed = tracking_speed;
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
}
else //front sensor invalid
{
Motor_stop();
return;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
}
else //front sensor invalid
{
// Motor_TurnRight(13, 13);
// delay_ms(50);
// Motor_stop();
Motor_stop();
return;
}
defl = fro_patt - back_patt;
if(defl < 0) // left deflection
{
Motor_TurnLeft(L_speed, R_speed);
}
if(defl > 0) //right deflection
{
Motor_TurnRight(L_speed, R_speed);
}
while(1)
{
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
defl = fro_patt - back_patt;
if (defl == 0)
break;
}
Motor_stop();
}
