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(); }