void init() { timer_init(); motor_init(); uart_init(); queue_init(); ultrasonic_init(); sei(); }
int main(void) { _delay_ms(250); servo_init(); ir_init(); bluetooth_init(); ultrasonic_init(); buttons_init(); sei(); while(1) { comm_exec(); move_proc(); if (button_get_state(1)) move_enable(false); if (button_get_state(2)) move_enable(true); } }
void navigation_update_state() { int32_t distance_values[N_ULTRASONIC_SENSORS]; int32_t smallest = 0; int32_t left_s = 0; int32_t right_s = 0; int32_t front1_s = 0; int32_t front2_s = 0; ultrasonic_get_distance(distance_values); smallest = ultrasonic_get_smallest(distance_values, N_ULTRASONIC_SENSORS); left_s = distance_values[US_LEFT]; right_s = distance_values[US_RIGHT]; front1_s = distance_values[US_FRONT_1]; front2_s = distance_values[US_FRONT_2]; if(navigation_status.current_state == BYPASS){ //motor command is done directly in CLI } else if(navigation_status.current_state == AVOID_OBSTACLE){ if (smallest > navigation_status.max_dist_obs){ navigation_status.current_state = GO_TO_TARGET; GPIO_write(Board_OBS_A_EN, 0); }else if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)){ navigation_status.current_state = SPACE_NEEDED; }else if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD){ serial_printf(cli_stdout, "Wall ahead!\n"); navigation_status.current_state = AVOID_WALL; } } else if (navigation_status.current_state == AVOID_WALL){ //We check if we're not facing the wall anymore if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)){ navigation_status.current_state = SPACE_NEEDED; } if (!((front1_s < BACKWARD_THRESHOLD) && (front2_s < BACKWARD_THRESHOLD))){ if (smallest > navigation_status.max_dist_obs){ navigation_status.current_state = GO_TO_TARGET; GPIO_write(Board_OBS_A_EN, 0); }else{ navigation_status.current_state = AVOID_OBSTACLE; } } } else if (navigation_status.current_state == SPACE_NEEDED){ if (!((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2))){ //Check if we face another type of obstacle or if path is free if (smallest < navigation_status.max_dist_obs){ //We use a different strategy depending on the type of obstacle if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD){ serial_printf(cli_stdout, "Wall ahead!\n"); navigation_status.current_state = AVOID_WALL; }else navigation_status.current_state = AVOID_OBSTACLE; }else{ navigation_status.current_state = GO_TO_TARGET; GPIO_write(Board_OBS_A_EN, 0); } } } else { //normal operation: continue on mission items if(mission_items.item[mission_items.current_index].current) { // Only go to target if mb is armed and rover must go to home OR sbc is armed and ready to record #ifdef CONTINUE_WAYPOINTS_IMMEDIATELY if(comm_mainboard_armed() == 1){ #else if(comm_mainboard_armed() == 1 && (mission_items.current_index == 0 || comm_sbc_armed() == 1)){ #endif navigation_status.current_state = GO_TO_TARGET; } else { navigation_status.current_state = STOP; } } if(navigation_status.current_state == GO_TO_TARGET) { ultrasonic_get_distance(distance_values); if(navigation_status.distance_to_target < TARGET_REACHED_DISTANCE) { navigation_mission_item_reached(); } else if (smallest < navigation_status.max_dist_obs) { GPIO_write(Board_OBS_A_EN, 1); //We use a different strategy depending on the type of obstacle if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD) { serial_printf(cli_stdout, "Wall ahead!\n"); navigation_status.current_state = AVOID_WALL; } else if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)) { navigation_status.current_state = SPACE_NEEDED; } else navigation_status.current_state = AVOID_OBSTACLE; } } } } #if defined (NAVIGATION_TEST) void navigation_move(); #else void navigation_move() { static int32_t lspeed, rspeed; double angular = 0; //only move if not on halt AND state = go_to_target if(navigation_status.halt == 0) { if(navigation_status.current_state == GO_TO_TARGET) { angular = pid_update(&pid_a, navigation_status.angle_to_target) * PID_SCALING_FACTOR; if (angular > 0){ lspeed = PWM_SPEED_100; rspeed = PWM_SPEED_100 - (int32_t)(angular); if (rspeed < PWM_MIN_CURVE_SPEED) rspeed = PWM_MIN_CURVE_SPEED; }else if (angular <= 0){ rspeed = PWM_SPEED_100; lspeed = PWM_SPEED_100 + (int32_t)(angular); if (lspeed < PWM_MIN_CURVE_SPEED) lspeed = PWM_MIN_CURVE_SPEED; } motors_wheels_move((int32_t)lspeed, (int32_t)rspeed, (int32_t)lspeed, (int32_t)rspeed); navigation_status.motor_values[0] = lspeed; //backup navigation_status.motor_values[1] = rspeed; } else if(navigation_status.current_state == STOP) { motors_wheels_stop(); navigation_status.motor_values[0] = 0; navigation_status.motor_values[1] = 0; } // else if(navigation_status.current_state == AVOID_OBSTACLE) // { // int32_t distance_values[N_ULTRASONIC_SENSORS]; // ultrasonic_get_distance(distance_values); // ultrasonic_check_distance(distance_values, navigation_status.motor_values, PWM_SPEED_80); //80% of speed to move slower than in normal mode // // motors_wheels_move(navigation_status.motor_values[0], navigation_status.motor_values[1], // navigation_status.motor_values[0], navigation_status.motor_values[1]); // }else if (navigation_status.current_state == AVOID_WALL){ // // move backwards in opposite curving direction (TODO: to be tested) // lspeed = -navigation_status.motor_values[1]; // rspeed = -navigation_status.motor_values[0]; // motors_wheels_move((int32_t)lspeed, (int32_t)rspeed, (int32_t)lspeed, (int32_t)rspeed); // }else if (navigation_status.current_state == SPACE_NEEDED){ // navigation_status.motor_values[0] = -PWM_SPEED_100; // navigation_status.motor_values[1] = -PWM_SPEED_100; // motors_wheels_move(navigation_status.motor_values[0], navigation_status.motor_values[1], // navigation_status.motor_values[0], navigation_status.motor_values[1]); // } } else { navigation_status.motor_values[0] = 0; navigation_status.motor_values[1] = 0; motors_wheels_stop(); } // serial_printf(cli_stdout, "speed l=%d, r=%d \n", motor_values[0], motor_values[1]); } #endif void navigation_change_gain(char pid, char type, float gain) { if (pid == 'a'){ if(type == 'p') pid_a.pGain = gain; else if (type == 'i') pid_a.iGain = gain; else if (type == 'd') pid_a.dGain = gain; } } #if defined (NAVIGATION_TEST) void navigation_init(); #else void navigation_init() { cli_init(); motors_init(); ultrasonic_init(); pid_init(&pid_a, PGAIN_A, IGAIN_A, DGAIN_A, MOTOR_IMAX, MOTOR_IMIN); navigation_status.lat_rover = INIT_LAT; navigation_status.lon_rover = INIT_LON; navigation_status.heading_rover = 0.0; navigation_status.lat_target = TARGET_LAT; navigation_status.lon_target = TARGET_LON; navigation_status.distance_to_target = 0.0; navigation_status.angle_to_target = 0.0; navigation_status.current_state = STOP; navigation_status.max_dist_obs = OBSTACLE_MAX_DIST; navigation_status.halt = 0; navigation_status.position_valid = false; GPIO_write(Board_OBS_A_EN, 1); mission_items.count = 0; }