Fully integrated ToF/IMU codes

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Revision:
11:73b4380d82bf
Parent:
10:b4d68db3ddbd
Child:
13:7fe71170d157
diff -r b4d68db3ddbd -r 73b4380d82bf main.cpp
--- a/main.cpp	Thu Jun 27 16:32:18 2019 +0000
+++ b/main.cpp	Fri Jun 28 21:18:51 2019 +0000
@@ -17,6 +17,10 @@
 AnalogIn x(A0);                     //Initializes analog axis for the joystick
 AnalogIn y(A1);
 
+double watchdogLimit = 0.1;             // Set timeout limit for watchdog timer in seconds
+int buttonCheck = 0;
+int iteration = 1;
+
 DigitalOut up(D12);                  //Turn up speed mode for joystick 
 DigitalOut down(D13);                //Turn down speed mode for joystick 
 DigitalOut on(D14);                 //Turn Wheelchair On
@@ -25,11 +29,11 @@
 
 Serial pc(USBTX, USBRX, 57600);     //Serial Monitor
 
-VL53L1X sensor1(PD_13, PD_12, PA_15);         //initializes ToF sensors
-VL53L1X sensor2(PD_13, PD_12, PC_7);
+VL53L1X sensor1(PD_13, PD_12, PC_7);         //initializes ToF sensors
+VL53L1X sensor2(PD_13, PD_12, PA_15);
 VL53L1X sensor3(PD_13, PD_12, PB_5);
-VL53L1X sensor4(PD_13, PD_12, PE_11);
-VL53L1X sensor5(PD_13, PD_12, PF_14);
+VL53L1X sensor4(PD_13, PD_12, PF_14);
+VL53L1X sensor5(PD_13, PD_12, PE_11);
 VL53L1X sensor6(PD_13, PD_12, PE_13);
 VL53L1X sensor7(PD_13, PD_12, D6);
 VL53L1X sensor8(PD_13, PD_12, PE_12);
@@ -58,12 +62,12 @@
     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
+    //queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);  //Sets up sampling frequency of the compass_thread
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread); //Sets up sampling frequency of the velosity_thread
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread); //Sets up sampling frequency of the velosity_thread
     //queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread
     t.reset();
-    compass.start(callback(&queue, &EventQueue::dispatch_forever));      //Starts running the compass thread
+    //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
     //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
@@ -132,8 +136,13 @@
             
             smart.stop();                                              //If nothing else is happening stop the chair
         }
-
+        
         wait(process);
+        
+/*        t.stop();
+        pc.printf("Time elapsed: %f seconds and iteration = %d\n", t.read(), iteration);
+        dog.Service();                                                  // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog
+    */
     }
 }