Diff: main.cpp

Revision:

14:88e5a437a58a

Parent:

13:1d0ed92043af

--- a/main.cpp	Thu Jun 27 18:33:05 2019 +0000
+++ b/main.cpp	Fri Jun 28 19:42:02 2019 +0000
@@ -10,14 +10,12 @@
 DigitalIn pt4(D9, PullUp);
 
 double watchdogLimit = 1.00;        // Set timeout limit for watchdog timer in milliseconds
-int buttonCheck = 0;
-
-/*added*/
-//DigitalIn e_button(D4);     //emergency button will start at HIGH
 
 QEI wheelS (D7, D8, NC, 1200);    //Initializes Left encoder
 DigitalIn pt1(D7, PullUp);          //Pull up resistors to read analog signals into digital signals
 DigitalIn pt2(D8, PullUp);
+//Added
+DigitalIn e_button(D6, PullDown);
 
 int max_velocity;
 //Timer testAccT;
@@ -25,10 +23,11 @@
 AnalogIn x(A0);                     //Initializes analog axis for the joystick
 AnalogIn y(A1);
 
-DigitalOut up(D12);                  //Turn up speed mode for joystick 
-DigitalOut down(D13);                //Turn down speed mode for joystick 
-DigitalOut on(D14);                 //Turn Wheelchair On
-DigitalOut off(D15);                //Turn Wheelchair Off
+DigitalOut up(D12);                  //Turn up speed mode for joystick
+DigitalOut down(D13);                //Turn down speed mode for joystick
+PwmOut on(PE_6);                     //Turn Wheelchair On
+PwmOut off(PE_5);                    //Turn Wheelchair Off
+
 bool manual = false;                //Turns chair joystic to automatic and viceverza
 
 Serial pc(USBTX, USBRX, 57600);     //Serial Monitor
@@ -39,15 +38,16 @@
 VL53L1X sensor4(PD_13, PD_12, PE_11);
 VL53L1X sensor5(PD_13, PD_12, PF_14);
 VL53L1X sensor6(PD_13, PD_12, PE_13);
-VL53L1X sensor7(PD_13, PD_12, D6);
+VL53L1X sensor7(PD_13, PD_12, D1);
 VL53L1X sensor8(PD_13, PD_12, PE_12);
 VL53L1X sensor9(PD_13, PD_12, PE_10);
 VL53L1X sensor10(PD_13, PD_12, PE_15);
-VL53L1X sensor11(PD_13, PD_12, D6);
+VL53L1X sensor11(PD_13, PD_12, D1);
 VL53L1X sensor12(PB_11, PB_10, D11);
 
-VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, 
-&sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12};                 //puts ToF sensor pointers into an array
+VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6,
+                    &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12
+                   };                 //puts ToF sensor pointers into an array
 VL53L1X** ToFT = ToF;
 
 Timer t;                            //Initialize time object t
@@ -59,11 +59,17 @@
 Thread emergencyButton;             // Thread to check button state and reset device
 
 int main(void)
-{   
-/*    nh.initNode();
-    nh.advertise(chatter);
-    nh.advertise(chatter2);
-    nh.subscribe(sub);*/
+{
+
+
+    Watchdog dog;                                                          // Creates Watchdog object
+    dog.Configure(watchdogLimit);                                          // Configures timeout for Watchdog
+    pc.printf("Code initiated/reset");
+
+    /*    nh.initNode();
+        nh.advertise(chatter);
+        nh.advertise(chatter2);
+        nh.subscribe(sub);*/
     //testAccT.start();
     pc.printf("before starting\r\n");
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);  //Sets up sampling frequency of the compass_thread
@@ -75,19 +81,11 @@
     compass.start(callback(&queue, &EventQueue::dispatch_forever));      //Starts running the compass thread
     velocity.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
     assistSafe.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
-    emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever)); 
-    
-    //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
-        pc.printf("After starting\r\n");
+    emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever));
 
-    //added
- //   int emerg_button = e_button;
-  
- Watchdog dog;                                                          // Creates Watchdog object
- dog.Configure(watchdogLimit);                                          // Configures timeout for Watchdog
- pc.printf("Code initiated/reset");
- 
- 
+    //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
+    pc.printf("After starting\r\n");
+
     int set = 0;
     while(1) {
         if( pc.readable()) {
@@ -96,20 +94,17 @@
             if( c == 'w') {
                 smart.forward();                                        //Move foward
 
-            }
-            else if( c == 'a') {
+            } else if( c == 'a') {
                 smart.left();                                           //Turn left
-            }
-            else if( c == 'd') {
+            } else if( c == 'd') {
                 smart.right();                                          //Turn right
-            }
-            else if( c == 's') {
+            } else if( c == 's') {
                 smart.backward();                                       //Turn rackwards
             }
-            
-            else if( c == 't') {                                        
+
+            else if( c == 't') {
                 smart.pid_twistA();
-            } else if(c == 'v'){
+            } else if(c == 'v') {
                 smart.showOdom();
             } else if(c == 'o') {                                       //Turns on chair
                 pc.printf("turning on\r\n");
@@ -121,8 +116,8 @@
                 off = 1;
                 wait(1);
                 off = 0;
-            
-            } else if(c == 'k'){                                        //Sends command to go to the kitchen
+
+            } else if(c == 'k') {                                       //Sends command to go to the kitchen
                 smart.pid_twistV();
             } else if( c == 'm' || manual) {                            //Turns wheelchair to joystick
                 pc.printf("turning on joystick\r\n");
@@ -137,15 +132,13 @@
                             manual = false;
                         }
                     }
-                }   
+                }
+            } else {
+                pc.printf("none \r\n");
+                smart.stop();                                      //If nothing else is happening stop the chair
             }
-             else {
-                    pc.printf("none \r\n");
-                    smart.stop();                                      //If nothing else is happening stop the chair
-            }
-        }
-        else {
-            
+        } else {
+
             smart.stop();                                              //If nothing else is happening stop the chair
         }