Diff: state_control.h

Revision:

19:65fecaa2a387

Parent:

17:d2c98ebda90b

Child:

20:7dcdadbd7bc0

--- a/state_control.h	Tue Oct 26 22:56:10 2021 +0000
+++ b/state_control.h	Thu Oct 28 00:28:29 2021 +0000
@@ -2,12 +2,19 @@
     This file controls the three states of the car 
     
 */
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
 InterruptIn stop_button(PTD0);
 InterruptIn run_button(PTD5); 
 InterruptIn wait_button(PTA13);  
 DigitalOut red_led(PTC16);
 DigitalOut green_led(PTC13);
 DigitalOut yellow_led(PTC12);
+
+PinName const SDA = PTE25;
+PinName const SCL = PTE24;
+
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
    
 // Available states 
 enum states {STOP, WAIT, RUN}; 
@@ -16,7 +23,9 @@
 enum led_colors {RED, YELLOW, GREEN};
 
 void turn_leds_off(void){ yellow_led =0; green_led =0; red_led=0;};
-
+enum fault_list {CLEAR,OFF_TRACK, COLLISION, LOW_VOLTAGE};
+fault_list fault_type=CLEAR;
+volatile bool _fault = false;
 // turns an led 
 void turn_led(led_colors Color){
        turn_leds_off();
@@ -40,12 +49,13 @@
     disable_brakes();
     motor_enabled = true; 
     turn_led(GREEN); 
-    setpointLeft = 0.2;
-    setpointRight = 0.2;
+    setpointLeft = 0.1;
+    setpointRight = 0.1;
     };
 void _wait(void){
      // release brakes, turn on steering system, do not start the motor
-    
+    _fault =false; 
+    fault_type=CLEAR;
      // disable_brakes();
      disable_brakes(); 
     motor_enabled = false;
@@ -57,7 +67,7 @@
      
 states current_state = WAIT;    // By default the car waits
 states prev_state = WAIT;
-volatile bool _fault = false;
+
 // This function gets called when a button is pushed
 void state_update (void){ 
     current_state = (stop_button.read()==1 || _fault == true) ? STOP: ((run_button.read() ==1 && prev_state == WAIT) ? RUN : (wait_button.read() ==1 )? WAIT:  prev_state); 
@@ -68,9 +78,10 @@
         }
       prev_state = current_state;
     }
-enum fault_list {CLEAR,OFF_TRACK, BUMPER, LOW_VOLTAGE};
-fault_list fault_type=CLEAR;
+
 void fault_check(void){
+     _fault = false; 
+        fault_type = CLEAR;
     batteryVoltage = battInput * 3.3 * battDividerScalar;
     avgCellVoltage = batteryVoltage / 3.0;
     if (left_distance_sensor.read() < (0.700/3.3) && right_distance_sensor.read()*3.3 < (0.700/3.3))
@@ -78,11 +89,17 @@
           _fault = true;
           fault_type = OFF_TRACK;
         }
-    /* else if (avgCellVoltage <= 3.4){
+    if (avgCellVoltage <= 3.4){
          _fault = true;
          fault_type = LOW_VOLTAGE;
-         }*/
-    else {                         //we should only set _fault to false when the user manually clears a fault w a button
+         }
+         
+     if (abs(acc.getAccX() > 1.5)){
+             _fault = true;
+             fault_type = COLLISION;
+             }        
+   
+   /* else {                         //we should only set _fault to false when the user manually clears a fault w a button
         _fault = false; 
         fault_type = CLEAR;
         };