Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

state_control.h@9:5320c2dfb913, 2021-10-26 (annotated)

Committer:
aalawfi
Date:
Tue Oct 26 01:22:05 2021 +0000
Revision:
9:5320c2dfb913
Parent:
7:05ea1c004b49
Child:
10:b0999f69c775
-

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;
quarren42 6:d2bd68ba99c9 32 // TODO: Activate brakes. Example: DONE
quarren42 6:d2bd68ba99c9 33 // left_brakes_enabled = true;
aalawfi 2:c857935f928e 34 // right_brakes_enabled = true;
quarren42 6:d2bd68ba99c9 35 brakeLeftBool = true;
quarren42 6:d2bd68ba99c9 36 brakeRightBool = true;
aalawfi 5:636c3fe18aa8 37 motor_enabled = false;
aalawfi 2:c857935f928e 38 turn_led(RED);
aalawfi 2:c857935f928e 39
aalawfi 2:c857935f928e 40 };
aalawfi 2:c857935f928e 41 void _run(void){
aalawfi 2:c857935f928e 42 // TODO: realease brakes, start the motors
quarren42 6:d2bd68ba99c9 43 brakeLeftBool = false;
quarren42 6:d2bd68ba99c9 44 brakeLeftBool = false;
aalawfi 5:636c3fe18aa8 45 motor_enabled = true;
aalawfi 9:5320c2dfb913 46 setpointLeft = 0.4;
aalawfi 9:5320c2dfb913 47 setpointRight = 0.4;
aalawfi 2:c857935f928e 48 turn_led(GREEN);
aalawfi 2:c857935f928e 49 };
aalawfi 2:c857935f928e 50 void _wait(void){
aalawfi 2:c857935f928e 51 // release brakes, turn on steering system, do not start the motor
aalawfi 2:c857935f928e 52 // left_brakes_enabled = false;
aalawfi 2:c857935f928e 53 // right_brakes_enabled = false;
quarren42 6:d2bd68ba99c9 54 brakeLeftBool = false;
quarren42 6:d2bd68ba99c9 55 brakeLeftBool = false;
aalawfi 5:636c3fe18aa8 56 motor_enabled = false;
aalawfi 9:5320c2dfb913 57 setpointLeft = 0.0;
aalawfi 9:5320c2dfb913 58 setpointRight = 0.0;
aalawfi 2:c857935f928e 59 steering_enabled = true;
aalawfi 2:c857935f928e 60 turn_led(YELLOW);
aalawfi 2:c857935f928e 61 };
aalawfi 2:c857935f928e 62
aalawfi 2:c857935f928e 63 states current_state = WAIT; // By default the car waits
aalawfi 2:c857935f928e 64 states prev_state = WAIT;
aalawfi 2:c857935f928e 65 volatile bool _fault = false;
aalawfi 2:c857935f928e 66 // This function gets called when a button is pushed
aalawfi 2:c857935f928e 67 void state_update (void){
aalawfi 2:c857935f928e 68 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 69 switch(current_state) {
aalawfi 2:c857935f928e 70 case STOP : _stop(); break;
aalawfi 2:c857935f928e 71 case WAIT : _wait(); break;
aalawfi 2:c857935f928e 72 case RUN : _run(); break;
aalawfi 2:c857935f928e 73 }
aalawfi 2:c857935f928e 74 prev_state = current_state;
aalawfi 2:c857935f928e 75 }
aalawfi 2:c857935f928e 76 enum fault_list {CLEAR,OFF_TRACK, BUMPER, LOW_VOLTAGE};
aalawfi 2:c857935f928e 77 fault_list fault_type=CLEAR;
aalawfi 2:c857935f928e 78 void fault_check(void){
aalawfi 7:05ea1c004b49 79 batteryVoltage = battInput * 3.3 * battDividerScalar;
aalawfi 7:05ea1c004b49 80 avgCellVoltage = batteryVoltage / 3.0;
aalawfi 9:5320c2dfb913 81 if (left_distance_sensor.read() < (0.700/3.3) && right_distance_sensor.read()*3.3 < (0.700/3.3))
aalawfi 2:c857935f928e 82 {
aalawfi 2:c857935f928e 83 _fault = true;
aalawfi 2:c857935f928e 84 fault_type = OFF_TRACK;
aalawfi 2:c857935f928e 85 }
aalawfi 9:5320c2dfb913 86 /* else if (avgCellVoltage <= 3.4){
quarren42 6:d2bd68ba99c9 87 _fault = true;
quarren42 6:d2bd68ba99c9 88 fault_type = LOW_VOLTAGE;
aalawfi 9:5320c2dfb913 89 }*/
aalawfi 7:05ea1c004b49 90 else { //we should only set _fault to false when the user manually clears a fault w a button
aalawfi 7:05ea1c004b49 91 _fault = false;
aalawfi 7:05ea1c004b49 92 fault_type = CLEAR;
aalawfi 7:05ea1c004b49 93 };
aalawfi 7:05ea1c004b49 94
quarren42 6:d2bd68ba99c9 95
aalawfi 2:c857935f928e 96 /*
aalawfi 2:c857935f928e 97 else if (hit something){
aalawfi 2:c857935f928e 98
aalawfi 2:c857935f928e 99 _fault = true;
aalawfi 2:c857935f928e 100 fault_type = BUMPER;
aalawfi 2:c857935f928e 101 }
aalawfi 2:c857935f928e 102
aalawfi 2:c857935f928e 103 else if(low battery)
aalawfi 2:c857935f928e 104 _fault = true;
aalawfi 2:c857935f928e 105 fault_type = LOW_VOLTAGE;
aalawfi 2:c857935f928e 106 else {
aalawfi 2:c857935f928e 107 _faule =false;
aalawfi 2:c857935f928e 108 fault_type = CLEAR;
aalawfi 2:c857935f928e 109 };
aalawfi 2:c857935f928e 110 */
aalawfi 2:c857935f928e 111 //_fault=true;
aalawfi 2:c857935f928e 112
aalawfi 2:c857935f928e 113 state_update();
aalawfi 2:c857935f928e 114 };
aalawfi 2:c857935f928e 115