Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

state_control.h@23:4c743746533c, 2021-11-03 (annotated)

Committer:
aalawfi
Date:
Wed Nov 03 21:52:00 2021 +0000
Revision:
23:4c743746533c
Parent:
20:7dcdadbd7bc0
Child:
25:8bd029d58251
- Most recent 11/3/2021

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 */
quarren42 19:65fecaa2a387 5 #define MMA8451_I2C_ADDRESS (0x1d<<1)
quarren42 19:65fecaa2a387 6
aalawfi 4:8b1d52dab862 7 InterruptIn stop_button(PTD0);
aalawfi 4:8b1d52dab862 8 InterruptIn run_button(PTD5);
aalawfi 4:8b1d52dab862 9 InterruptIn wait_button(PTA13);
aalawfi 4:8b1d52dab862 10 DigitalOut red_led(PTC16);
aalawfi 4:8b1d52dab862 11 DigitalOut green_led(PTC13);
aalawfi 4:8b1d52dab862 12 DigitalOut yellow_led(PTC12);
quarren42 19:65fecaa2a387 13
quarren42 19:65fecaa2a387 14 PinName const SDA = PTE25;
quarren42 19:65fecaa2a387 15 PinName const SCL = PTE24;
quarren42 19:65fecaa2a387 16
quarren42 19:65fecaa2a387 17 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
aalawfi 2:c857935f928e 18
aalawfi 2:c857935f928e 19 // Available states
aalawfi 2:c857935f928e 20 enum states {STOP, WAIT, RUN};
aalawfi 2:c857935f928e 21
aalawfi 2:c857935f928e 22 // Available LED colors
aalawfi 2:c857935f928e 23 enum led_colors {RED, YELLOW, GREEN};
aalawfi 2:c857935f928e 24
aalawfi 2:c857935f928e 25 void turn_leds_off(void){ yellow_led =0; green_led =0; red_led=0;};
quarren42 19:65fecaa2a387 26 enum fault_list {CLEAR,OFF_TRACK, COLLISION, LOW_VOLTAGE};
quarren42 19:65fecaa2a387 27 fault_list fault_type=CLEAR;
quarren42 19:65fecaa2a387 28 volatile bool _fault = false;
aalawfi 2:c857935f928e 29 // turns an led
aalawfi 2:c857935f928e 30 void turn_led(led_colors Color){
aalawfi 2:c857935f928e 31 turn_leds_off();
aalawfi 2:c857935f928e 32 switch(Color) {
aalawfi 2:c857935f928e 33 case RED : {red_led = 1; green_led=0; yellow_led=0;}; break;
aalawfi 2:c857935f928e 34 case YELLOW : {yellow_led = 1; green_led =0; red_led=0;}; break;
aalawfi 2:c857935f928e 35 case GREEN : {green_led = 1; yellow_led =0; red_led=0;}; break;
aalawfi 2:c857935f928e 36 }
aalawfi 2:c857935f928e 37 };
aalawfi 2:c857935f928e 38
aalawfi 2:c857935f928e 39 void _stop(void){
aalawfi 2:c857935f928e 40 steering_enabled =false;
aalawfi 16:8cd4dd323941 41 enable_brakes();
aalawfi 5:636c3fe18aa8 42 motor_enabled = false;
aalawfi 2:c857935f928e 43 turn_led(RED);
aalawfi 2:c857935f928e 44
aalawfi 2:c857935f928e 45 };
aalawfi 2:c857935f928e 46 void _run(void){
aalawfi 12:f6139597354e 47 // TODO: realease brakes, start the motors
aalawfi 16:8cd4dd323941 48 //
quarren42 17:d2c98ebda90b 49 disable_brakes();
aalawfi 5:636c3fe18aa8 50 motor_enabled = true;
aalawfi 2:c857935f928e 51 turn_led(GREEN);
quarren42 19:65fecaa2a387 52 setpointLeft = 0.1;
quarren42 19:65fecaa2a387 53 setpointRight = 0.1;
aalawfi 2:c857935f928e 54 };
aalawfi 2:c857935f928e 55 void _wait(void){
aalawfi 2:c857935f928e 56 // release brakes, turn on steering system, do not start the motor
quarren42 19:65fecaa2a387 57 _fault =false;
quarren42 19:65fecaa2a387 58 fault_type=CLEAR;
aalawfi 12:f6139597354e 59 // disable_brakes();
aalawfi 16:8cd4dd323941 60 disable_brakes();
aalawfi 5:636c3fe18aa8 61 motor_enabled = false;
aalawfi 9:5320c2dfb913 62 setpointLeft = 0.0;
aalawfi 9:5320c2dfb913 63 setpointRight = 0.0;
aalawfi 2:c857935f928e 64 steering_enabled = true;
aalawfi 2:c857935f928e 65 turn_led(YELLOW);
aalawfi 2:c857935f928e 66 };
aalawfi 2:c857935f928e 67
aalawfi 2:c857935f928e 68 states current_state = WAIT; // By default the car waits
aalawfi 2:c857935f928e 69 states prev_state = WAIT;
quarren42 19:65fecaa2a387 70
aalawfi 2:c857935f928e 71 // This function gets called when a button is pushed
aalawfi 2:c857935f928e 72 void state_update (void){
aalawfi 2:c857935f928e 73 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 74 switch(current_state) {
aalawfi 2:c857935f928e 75 case STOP : _stop(); break;
aalawfi 2:c857935f928e 76 case WAIT : _wait(); break;
aalawfi 2:c857935f928e 77 case RUN : _run(); break;
aalawfi 2:c857935f928e 78 }
aalawfi 2:c857935f928e 79 prev_state = current_state;
aalawfi 2:c857935f928e 80 }
quarren42 19:65fecaa2a387 81
aalawfi 2:c857935f928e 82 void fault_check(void){
quarren42 20:7dcdadbd7bc0 83 _fault = false;
quarren42 19:65fecaa2a387 84 fault_type = CLEAR;
aalawfi 7:05ea1c004b49 85 batteryVoltage = battInput * 3.3 * battDividerScalar;
aalawfi 7:05ea1c004b49 86 avgCellVoltage = batteryVoltage / 3.0;
quarren42 20:7dcdadbd7bc0 87
aalawfi 23:4c743746533c 88 if (left_distance_sensor.read() < (0.600/3.3) && right_distance_sensor.read()*3.3 < (0.600/3.3))
aalawfi 2:c857935f928e 89 {
aalawfi 2:c857935f928e 90 _fault = true;
aalawfi 2:c857935f928e 91 fault_type = OFF_TRACK;
aalawfi 2:c857935f928e 92 }
quarren42 20:7dcdadbd7bc0 93
quarren42 20:7dcdadbd7bc0 94
quarren42 19:65fecaa2a387 95 if (avgCellVoltage <= 3.4){
quarren42 6:d2bd68ba99c9 96 _fault = true;
quarren42 6:d2bd68ba99c9 97 fault_type = LOW_VOLTAGE;
aalawfi 23:4c743746533c 98 }
aalawfi 23:4c743746533c 99 if (abs(acc.getAccX() > 1.48)){ // was 1.5
aalawfi 23:4c743746533c 100 _fault = true;
aalawfi 23:4c743746533c 101 fault_type = COLLISION;
aalawfi 23:4c743746533c 102 }
quarren42 20:7dcdadbd7bc0 103
aalawfi 2:c857935f928e 104
aalawfi 2:c857935f928e 105 state_update();
aalawfi 2:c857935f928e 106 };
aalawfi 2:c857935f928e 107