void EXTI1_IRQHandler(){ if(EXTI_GetITStatus(EXTI_Line1) != RESET) { speed_right_counter++ ; distance_right_counter ++; if(car_state==CAR_STATE_MOVE_LEFT){ if (distance_right_counter >= MOVE_LEFT_RIGHT_PERIOD && distance_left_counter >= MOVE_LEFT_RIGHT_PERIOD) //2000 == 4 cycle { controller_reset(&n_r); controller_reset(&n_l_back); car_state = CAR_STATE_IDLE; last_state = CAR_STATE_MOVE_LEFT; } } else if(car_state==CAR_STATE_MOVE_RIGHT){ if(distance_right_counter >= MOVE_LEFT_RIGHT_PERIOD && distance_left_counter >= MOVE_LEFT_RIGHT_PERIOD) { controller_reset(&n_r_back); controller_reset(&n_l); car_state = CAR_STATE_IDLE; last_state = CAR_STATE_MOVE_RIGHT; } } EXTI_ClearITPendingBit(EXTI_Line1); } }
void controller_handler(const double xk, double *yk) { double ek; double p_term, i_term, d_term; ek = p_param.t_target - xk; /* error */ if (ek >= 3.0) { syslog(LOG_DEBUG, "PID: %3.1f Below set point %3.1f, stop\n", xk, p_param.t_target); controller_reset(); *yk = 0.0; return; } /* compute intermediate PID terms */ p_term = -p_param.kp * (xk - xk_1); i_term = p_param.kp * p_param.ki * p_param.ts * ek; d_term = -p_param.kp * p_param.kd * (xk - 2 * xk_1 + xk_2) / p_param.ts; /* compute output */ *yk += p_term + i_term + d_term; /* update sample data */ xk_1 = xk; xk_2 = xk_1; /* clamp output adjustment range */ if (*yk < -LIMIT_HIGH) *yk = -LIMIT_HIGH; else if (*yk > -LIMIT_LOW) *yk = -LIMIT_LOW; p_param.y_k = *yk; set_ctrl_state(lround(fabs(p_param.y_k))); }
static void setup_pathfin(TinBot* tinbot) { hal_print("Tin Bot Setup: Path Finder (20,30), somewhat"); controller_reset(&tinbot->controller, &tinbot->sens); /* First, persuade the Moderator that we're already rescuing the victim * all along, and that this is fine. */ tinbot->controller.moderator.locals.state = 4; tinbot->rx_buffer.moderate.owning_xy_p = 1; /* Call ProxFilter, for fun. * Also, disable TCE's interrupt. */ tinbot->controller.moderator.found_victim_xy = 1; /* approx_step can run freely, as approx_reset was called by * controller_reset. */ /* Next, Blind Cop has lots of internal state. */ tinbot->controller.blind.locals.state_big = 1; tinbot->controller.blind.locals.state_leaf = 5; /* Finally, tell the controller where to go. */ tinbot->controller.first_iter = 0; tinbot->controller.origin.x = 20.0; tinbot->controller.origin.y = 30.0; /* The rest should be propagated automatically. */ }
static bool state_machine_filter(enum state_function *state) { switch (*state) { case STATE_FCT_IDLE: if (new_state_fct) { gui_set_selection(GUI_FILTER_ID); gui_text_print(GUI_COMMENT_ID, TEXT_FILTER); } else { if (controller_wheel_right(2)) switch_state(STATE_OUTPUT); else if (controller_wheel_left(2)) switch_state(STATE_INPUT); return false; } break; // FFT case STATE_FCT_FUNCTION1: if (new_state_fct) filter_set_active(0); break; case STATE_FCT_FUNCTION2: if (new_state_fct) filter_set_active(1); break; case STATE_FCT_FUNCTION3: if (new_state_fct) filter_set_active(2); break; case STATE_FCT_ZOOM: break; } if (new_state_fct) gui_text_print(GUI_FILTER_ID, filter_active_get_description()); if (controller_wheel_right(0) || controller_wheel_left(0)) { *state = STATE_FCT_IDLE; controller_reset(); } return true; }
int main( void ) { controller_reset( "localhost", 2002 ); controller_set( 0, 0, -5, 0 ); while( 1 ) { double north; double east; double down; double roll; double pitch; double yaw; int rc; rc = controller_step( &north, &east, &down, &roll, &pitch, &yaw ); if( rc == 0 ) continue; if( rc < 0 ) break; fprintf( stderr, "(%2.3f,%2.3f,%2.3f,%2.3f)\r", north, east, down, yaw ); } fprintf( stderr, "\n" ); return 0; }
static void state_machine_task(void) { bool is_reset; // Set function state if (controller_key_fct3_pressed()) { state_fct = STATE_FCT_ZOOM; new_state_fct = true; } else if (controller_key_fct1()) { state_fct = STATE_FCT_FUNCTION1; new_state_fct = true; } else if (controller_key_fct2()) { state_fct = STATE_FCT_FUNCTION2; new_state_fct = true; } else if (controller_key_fct3()) { state_fct = STATE_FCT_FUNCTION3; new_state_fct = true; } // Clear Zoom state if on and a key is pressed if (zoom_view && !controller_key_fct3_pressed()) { zoom_view = false; gui_clear_view(); gui_text_print(GUI_FILTER_ID, filter_active_get_description()); new_state_fct = true; state_fct = STATE_FCT_IDLE; controller_key_reset(); } // Reset or back modes is_reset = controller_key_reset(); if ((controller_key_back() && !zoom_view) || is_reset) { if (is_reset) { // SW resetof the application Reset_CPU(); while(1); } if (state_fct == STATE_FCT_IDLE) state = STATE_IDLE; state_fct = STATE_FCT_IDLE; // Reset the controller (delete saved key states) controller_reset(); new_state_fct = true; } switch (state) { case STATE_IDLE: if (!state_machine_idle(&state_fct)) return; break; case STATE_SOURCE1: if (!state_machine_source(1, &state_fct)) return; break; case STATE_SOURCE2: if (!state_machine_source(2, &state_fct)) return; break; case STATE_INPUT: if (!state_machine_signal(STATE_INPUT, &state_fct)) return; break; case STATE_FILTER: if (!state_machine_filter(&state_fct)) return; break; case STATE_OUTPUT: if (!state_machine_signal(STATE_OUTPUT, &state_fct)) return; break; } new_state_fct = false; }
static bool state_machine_signal(enum state_master id, enum state_function *state) { switch (*state) { case STATE_FCT_IDLE: if (id == STATE_INPUT) { if (new_state_fct) { gui_set_selection(GUI_INPUT_ID); gui_text_print(GUI_COMMENT_ID, TEXT_INPUT); } else { if (controller_wheel_right(2)) switch_state(STATE_FILTER); else if (controller_wheel_left(2)) switch_state(STATE_SOURCE2); return false; } } else if (id == STATE_OUTPUT) { if (new_state_fct) { gui_set_selection(GUI_OUTPUT_ID); gui_text_print(GUI_COMMENT_ID, TEXT_OUTPUT); } else { if (controller_wheel_left(2)) switch_state(STATE_FILTER); return false; } } break; // FFT case STATE_FCT_FUNCTION1: if (new_state_fct) { if (id == STATE_INPUT) { input_fft_view = !input_fft_view; if (input_fft_view) gui_text_print(GUI_COMMENT_ID, "Input - " TEXT_FUNC1 "\nFrequency domain\n\n\n\n(Use " TEXT_FUNC1 ")"); else gui_text_print(GUI_COMMENT_ID, "Input - " TEXT_FUNC1 "\nTemporal domain\n\n\n\n(Use " TEXT_FUNC1 ")"); } else if (id == STATE_OUTPUT) { output_fft_view = !output_fft_view; if (output_fft_view) gui_text_print(GUI_COMMENT_ID, "Output - " TEXT_FUNC1 "\nFrequency domain\n\n\n\n(Use " TEXT_FUNC1 ")"); else gui_text_print(GUI_COMMENT_ID, "Output - " TEXT_FUNC1 "\nTemporal domain\n\n\n\n(Use " TEXT_FUNC1 ")"); } } break; case STATE_FCT_FUNCTION2: case STATE_FCT_FUNCTION3: if (new_state_fct) gui_text_print(GUI_COMMENT_ID, TEXT_FUNC_NOT_IMPLEMENTED); break; // Zoom case STATE_FCT_ZOOM: if (new_state_fct) { zoom_view = true; if (id == STATE_INPUT) zoom_view_id = GUI_INPUT_ID; else if (id == STATE_OUTPUT) zoom_view_id = GUI_OUTPUT_ID; // controller_reset(); } break; } if (controller_wheel_right(0) || controller_wheel_left(0)) { *state = STATE_FCT_IDLE; controller_reset(); } return true; }
static bool state_machine_source(int source_id, enum state_function *state) { static dsp16_t volume; static unsigned int frequency; struct signal_source *source = NULL; if (source_id == 1) source = &signal1_generator; else if (source_id == 2) source = &signal2_generator; switch (*state) { case STATE_FCT_IDLE: if (source_id == 1) { if (new_state_fct) { gui_set_selection(GUI_SOURCE1_ID); gui_text_print(GUI_COMMENT_ID, TEXT_SOURCE1); } else { if (controller_wheel_right(2)) switch_state(STATE_SOURCE2); return false; } } else if (source_id == 2) { if (new_state_fct) { gui_set_selection(GUI_SOURCE2_ID); gui_text_print(GUI_COMMENT_ID, TEXT_SOURCE2); } else { if (controller_wheel_left(2)) switch_state(STATE_SOURCE1); else if (controller_wheel_right(2)) switch_state(STATE_INPUT); return false; } } break; // Amplitude case STATE_FCT_FUNCTION1: volume = signal_source_get_volume(source); if (controller_wheel_right(1) && volume < DSP16_Q(1.)) { if (volume < DSP16_Q(1.) - DSP16_Q(1./16)) volume += DSP16_Q(1./16); else volume = DSP16_Q(1.); new_state_fct = true; } else if (controller_wheel_left(1)) { if (volume > DSP16_Q(1./16)) volume -= DSP16_Q(1./16); else volume = 0; new_state_fct = true; } if (new_state_fct) { signal_source_set_volume(source, volume); gui_text_printf(GUI_COMMENT_ID, "Source%i - " TEXT_FUNC1 "\nAmplitude %f\n\n\n\n" TEXT_WHEEL, source_id, volume); } break; // Frequency case STATE_FCT_FUNCTION2: frequency = signal_source_get_freq(source); if (controller_wheel_right(1) && frequency < 10000) { frequency *= 1.1; new_state_fct = true; } else if (controller_wheel_left(1) && frequency > 100) { frequency *= 0.9; new_state_fct = true; } if (new_state_fct) { signal_source_set_freq(source, frequency); gui_text_printf(GUI_COMMENT_ID, "Source%i - " TEXT_FUNC2 "\nFrequency %iHz\n\n\n\n" TEXT_WHEEL, source_id, frequency); } break; case STATE_FCT_FUNCTION3: break; // Zoom case STATE_FCT_ZOOM: if (new_state_fct) { zoom_view = true; if (source_id == 1) zoom_view_id = GUI_SOURCE1_ID; else if (source_id == 2) zoom_view_id = GUI_SOURCE2_ID; controller_reset(); } break; } return true; }
void neural_task(void *p) { while(1){ detachInterrupt(EXTI_Line0); /*close external interrupt 0*/ detachInterrupt(EXTI_Line1); /*close external interrupt 1*/ detachInterrupt(EXTI_Line2); /*close external interrupt 2*/ detachInterrupt(EXTI_Line3); /*close external interrupt 3*/ if(car_state == CAR_STATE_MOVE_FORWARD){ getMotorData(); rin = move_speed; neural_update(&n_r, rin, speed_right_counter_1, distance_right_counter); neural_update(&n_l, rin, speed_left_counter_1, distance_left_counter); data_record(); float input_l = n_l.u; float input_r = n_r.u; proc_cmd("forward", base_pwm_l+(int)input_l, base_pwm_r+(int)input_r); } else if (car_state == CAR_STATE_MOVE_BACK) { getMotorData(); float err = 0; rin = 10; neural_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); neural_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); float input_l = n_l_back.u_1; float input_r = n_r_back.u_1; proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r); } else if (car_state == CAR_STATE_MOVE_LEFT){ rin = 5; getMotorData(); if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); }else{ pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); } if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); }else{ pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); } float input_l = n_l_back.u; float input_r = n_r.u; proc_cmd("left", base_pwm_l-(int)input_l, base_pwm_r+(int)input_r); } else if (car_state == CAR_STATE_MOVE_RIGHT){ rin = 5; getMotorData(); if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); }else{ pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); } if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); }else{ pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); } float input_l = n_l.u; float input_r = n_r_back.u; proc_cmd("right", base_pwm_l+(int)input_l, base_pwm_r-(int)input_r); } else if(car_state == CAR_STATE_STOPPING){ getMotorData(); if (last_state == CAR_STATE_MOVE_FORWARD) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l.u; float input_r = n_r.u; proc_cmd("forward", base_pwm_l + input_l, base_pwm_r + input_r); } else{ record_controller_base(&n_r); record_controller_base(&n_l); controller_reset(&n_r); controller_reset(&n_l); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } else if (last_state == CAR_STATE_MOVE_BACK) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l_back.u; float input_r = n_r_back.u; proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r); } else{ record_controller_base(&n_r_back); record_controller_base(&n_l_back); controller_reset(&n_r_back); controller_reset(&n_l_back); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } else if (last_state == CAR_STATE_MOVE_LEFT) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l_back.u; float input_r = n_r.u; proc_cmd("forward", base_pwm_l - input_l, base_pwm_r + input_r); } else{ //record_controller_base(&n_r); //record_controller_base(&n_l_back); controller_reset(&n_r); controller_reset(&n_l_back); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } else if (last_state == CAR_STATE_MOVE_RIGHT) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l.u; float input_r = n_r_back.u; proc_cmd("forward", base_pwm_l + input_l, base_pwm_r - input_r); } else{ //record_controller_base(&n_r_back); //record_controller_base(&n_l); controller_reset(&n_r_back); controller_reset(&n_l); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } } else if(car_state == CAR_STATE_IDLE){ proc_cmd("forward", base_pwm_l, base_pwm_r); speeed_initialize(); distance_initialize(); } else{ } attachInterrupt(EXTI_Line0); attachInterrupt(EXTI_Line1); attachInterrupt(EXTI_Line2); attachInterrupt(EXTI_Line3); vTaskDelay(NEURAL_PERIOD); } }
/* Mode - Full */ static void setup_full(TinBot* tinbot) { hal_print("Tin Bot Setup: FULL"); controller_reset(&tinbot->controller, &tinbot->sens); }