Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

state_control.h@5:636c3fe18aa8, 2021-10-25 (annotated)

Committer:
aalawfi
Date:
Mon Oct 25 02:14:11 2021 +0000
Revision:
5:636c3fe18aa8
Parent:
4:8b1d52dab862
Child:
6:d2bd68ba99c9
- Attempted to extend state control to driving: I wrapped the PI controller in an if-else statement, if the motor is enabled, allow PI controll, if the motor is disabled, write duty cycle of zero.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aalawfi 2:c857935f928e 1 /*
aalawfi 2:c857935f928e 2 This file controls the three states of the car
aalawfi 2:c857935f928e 3
aalawfi 2:c857935f928e 4 */
aalawfi 4:8b1d52dab862 5 InterruptIn stop_button(PTD0);
aalawfi 4:8b1d52dab862 6 InterruptIn run_button(PTD5);
aalawfi 4:8b1d52dab862 7 InterruptIn wait_button(PTA13);
aalawfi 4:8b1d52dab862 8 DigitalOut red_led(PTC16);
aalawfi 4:8b1d52dab862 9 DigitalOut green_led(PTC13);
aalawfi 4:8b1d52dab862 10 DigitalOut yellow_led(PTC12);
aalawfi 2:c857935f928e 11
aalawfi 2:c857935f928e 12 // Available states
aalawfi 2:c857935f928e 13 enum states {STOP, WAIT, RUN};
aalawfi 2:c857935f928e 14
aalawfi 2:c857935f928e 15 // Available LED colors
aalawfi 2:c857935f928e 16 enum led_colors {RED, YELLOW, GREEN};
aalawfi 2:c857935f928e 17
aalawfi 2:c857935f928e 18 void turn_leds_off(void){ yellow_led =0; green_led =0; red_led=0;};
aalawfi 2:c857935f928e 19
aalawfi 2:c857935f928e 20 // turns an led
aalawfi 2:c857935f928e 21 void turn_led(led_colors Color){
aalawfi 2:c857935f928e 22 turn_leds_off();
aalawfi 2:c857935f928e 23 switch(Color) {
aalawfi 2:c857935f928e 24 case RED : {red_led = 1; green_led=0; yellow_led=0;}; break;
aalawfi 2:c857935f928e 25 case YELLOW : {yellow_led = 1; green_led =0; red_led=0;}; break;
aalawfi 2:c857935f928e 26 case GREEN : {green_led = 1; yellow_led =0; red_led=0;}; break;
aalawfi 2:c857935f928e 27 }
aalawfi 2:c857935f928e 28 };
aalawfi 2:c857935f928e 29
aalawfi 2:c857935f928e 30 void _stop(void){
aalawfi 2:c857935f928e 31 steering_enabled =false;
aalawfi 2:c857935f928e 32 // TODO: Activate brakes. Example:
aalawfi 2:c857935f928e 33 // left_brakes_enabled = true;
aalawfi 2:c857935f928e 34 // right_brakes_enabled = true;
aalawfi 5:636c3fe18aa8 35 motor_enabled = false;
aalawfi 2:c857935f928e 36 turn_led(RED);
aalawfi 2:c857935f928e 37
aalawfi 2:c857935f928e 38 };
aalawfi 2:c857935f928e 39 void _run(void){
aalawfi 2:c857935f928e 40 // TODO: realease brakes, start the motors
aalawfi 5:636c3fe18aa8 41 motor_enabled = true;
aalawfi 2:c857935f928e 42 turn_led(GREEN);
aalawfi 2:c857935f928e 43 };
aalawfi 2:c857935f928e 44 void _wait(void){
aalawfi 2:c857935f928e 45 // release brakes, turn on steering system, do not start the motor
aalawfi 2:c857935f928e 46 // left_brakes_enabled = false;
aalawfi 2:c857935f928e 47 // right_brakes_enabled = false;
aalawfi 5:636c3fe18aa8 48 motor_enabled = false;
aalawfi 2:c857935f928e 49 steering_enabled = true;
aalawfi 2:c857935f928e 50 turn_led(YELLOW);
aalawfi 2:c857935f928e 51 };
aalawfi 2:c857935f928e 52
aalawfi 2:c857935f928e 53 states current_state = WAIT; // By default the car waits
aalawfi 2:c857935f928e 54 states prev_state = WAIT;
aalawfi 2:c857935f928e 55 volatile bool _fault = false;
aalawfi 2:c857935f928e 56 // This function gets called when a button is pushed
aalawfi 2:c857935f928e 57 void state_update (void){
aalawfi 2:c857935f928e 58 current_state = (stop_button.read()==1 || _fault == true) ? STOP: ((run_button.read() ==1 && prev_state == WAIT) ? RUN : (wait_button.read() ==1 )? WAIT: prev_state);
aalawfi 2:c857935f928e 59 switch(current_state) {
aalawfi 2:c857935f928e 60 case STOP : _stop(); break;
aalawfi 2:c857935f928e 61 case WAIT : _wait(); break;
aalawfi 2:c857935f928e 62 case RUN : _run(); break;
aalawfi 2:c857935f928e 63 }
aalawfi 2:c857935f928e 64 prev_state = current_state;
aalawfi 2:c857935f928e 65 }
aalawfi 2:c857935f928e 66 enum fault_list {CLEAR,OFF_TRACK, BUMPER, LOW_VOLTAGE};
aalawfi 2:c857935f928e 67 fault_list fault_type=CLEAR;
aalawfi 2:c857935f928e 68 void fault_check(void){
aalawfi 2:c857935f928e 69 if (left_distance_sensor.read()*3.3 < 0.700 && right_distance_sensor.read()*3.3 < 0.700)
aalawfi 2:c857935f928e 70 {
aalawfi 2:c857935f928e 71 _fault = true;
aalawfi 2:c857935f928e 72 fault_type = OFF_TRACK;
aalawfi 2:c857935f928e 73 }
aalawfi 2:c857935f928e 74 else {
aalawfi 2:c857935f928e 75 _fault = false;
aalawfi 2:c857935f928e 76 fault_type = CLEAR;
aalawfi 2:c857935f928e 77 };
aalawfi 2:c857935f928e 78
aalawfi 2:c857935f928e 79 /*
aalawfi 2:c857935f928e 80 else if (hit something){
aalawfi 2:c857935f928e 81
aalawfi 2:c857935f928e 82 _fault = true;
aalawfi 2:c857935f928e 83 fault_type = BUMPER;
aalawfi 2:c857935f928e 84 }
aalawfi 2:c857935f928e 85
aalawfi 2:c857935f928e 86 else if(low battery)
aalawfi 2:c857935f928e 87 _fault = true;
aalawfi 2:c857935f928e 88 fault_type = LOW_VOLTAGE;
aalawfi 2:c857935f928e 89 else {
aalawfi 2:c857935f928e 90 _faule =false;
aalawfi 2:c857935f928e 91 fault_type = CLEAR;
aalawfi 2:c857935f928e 92 };
aalawfi 2:c857935f928e 93 */
aalawfi 2:c857935f928e 94 //_fault=true;
aalawfi 2:c857935f928e 95
aalawfi 2:c857935f928e 96 state_update();
aalawfi 2:c857935f928e 97 };
aalawfi 2:c857935f928e 98