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