void hugOpstacle() { while(!(inRange(440, 20) && turnCount == 0)) { int right = Sensor_getDistance(Sensor_RIGHT); int left = Sensor_getDistance(Sensor_LEFT); int front = (Sensor_getDistance(Sensor_FRONT_LEFT) + Sensor_getDistance(Sensor_FRONT_RIGHT)) / 2; if(right < 10) Motor_steerLeft(0.9); else if(rechts < 30) { if(voor > 20) Motor_Forward(); else Motor_TurnLeft(); } else if(rechts < 40) Motor_steerRight(0.9); else Motor_turnRight(); calculateTurnCount(); } }
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 Golfer_turn_left_special() { 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(back_sensor==0b00111000 || back_sensor==0b00011100 || back_sensor==0b00011000) break; } Motor_stop(); // Motor_forward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR); // while((middle_sensor & 0b00000010) == 0b00000000) { ; } // Motor_stop(); Motor_TurnLeft(L_speed - 1, R_speed + 1); while(1) { if((front_sensor & 0b00011000) == 0) break; } while(1) { if(front_sensor == 0b00111000 || front_sensor == 0b00011000) { break; } } Motor_stop(); }
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(); }