Diff: main.cpp

Revision:

20:bd257ffdef24

Parent:

19:7ead04523f89

Child:

21:b438961f85d5

--- a/main.cpp	Tue Jul 02 17:53:42 2019 +0000
+++ b/main.cpp	Tue Jul 02 18:53:53 2019 +0000
@@ -1,95 +1,118 @@
-/**************************
-*       Version 1.0       *
-**************************/
+/*************************************************************************
+ *                             Version 1.0                               *
+ *************************************************************************/
+#include "wheelchair.h"
+Serial pc(USBTX, USBRX, 57600);         // Serial Monitor
 
-#include "wheelchair.h"
-
+/**************************************************************************
+ *                      Encoder Pins & Variables                          *
+ **************************************************************************/
+ 
 QEI wheel (D10, D9, NC, 1200);          // Initializes right encoder
 DigitalIn pt3(D10, PullUp);             // Pull up resistors to read analog signals into digital signals
 DigitalIn pt4(D9, PullUp);
 
-DigitalIn e_button(D6, PullDown);       // Change to PullUp if testing without Emergency Button Connected
-
 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);
 
 int max_velocity;
 
-AnalogIn x(A0);                         // Initializes analog axis for the joystick
-AnalogIn y(A1);
-
+/**************************************************************************
+ *        Variables and Pins for Watchdog and Emergency Button            *
+ **************************************************************************/
+ 
+DigitalIn e_button(D6, PullDown);       // Change to PullUp if testing without Emergency Button Connected
+PwmOut on(PE_6);                        // Turn Wheelchair On
+PwmOut off(PE_5);                       // Turn Wheelchair Off
 
 //Watchdog limit should be 0.1; Set to 1 for Testing Only
 double watchdogLimit = 1;               // Set timeout limit for watchdog timer in seconds
 int buttonCheck = 0;
 int iteration = 1;
 
+/**************************************************************************
+ *                      Joystick Pins & Variables                         *
+ **************************************************************************/
+ 
+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 
-
-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
+/**************************************************************************
+ *                      ToF Sensor Pin Assignments                        *
+ **************************************************************************/
 
-VL53L1X sensor1(PD_13, PD_12, PC_7);    // Initializes ToF sensors
-VL53L1X sensor2(PD_13, PD_12, PA_15);
+VL53L1X sensor1(PD_13, PD_12, PA_15);   // Block 1
+VL53L1X sensor2(PD_13, PD_12, PC_7);
 VL53L1X sensor3(PD_13, PD_12, PB_5);
-VL53L1X sensor4(PD_13, PD_12, PF_14);
-VL53L1X sensor5(PD_13, PD_12, PE_11);
+
+VL53L1X sensor4(PD_13, PD_12, PE_11);   // Block 2
+VL53L1X sensor5(PD_13, PD_12, PF_14);
 VL53L1X sensor6(PD_13, PD_12, PE_13);
-VL53L1X sensor7(PD_13, PD_12, D6);
-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 sensor12(PB_11, PB_10, D11);
+
+VL53L1X sensor7(PD_13, PD_12, PG_15);   // Block 3
+VL53L1X sensor8(PD_13, PD_12, PG_14);
+VL53L1X sensor9(PD_13, PD_12, PG_9);
+
+VL53L1X sensor10(PD_13, PD_12, PE_10);  // Block 4
+VL53L1X sensor11(PD_13, PD_12, PE_12);     
+VL53L1X sensor12(PD_13, PD_12, PE_14);
 
 VL53L1X* ToF[12] = {&sensor1, &sensor2, &sensor3, &sensor4, &sensor5, &sensor6, 
-&sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12};                 // Puts ToF sensor pointers into an array
+&sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; // Puts ToF sensor pointers into an array
 VL53L1X** ToFT = ToF;
 
-Timer t;                                // Initialize time object t
-EventQueue queue;                       // Class to organize threads
-Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT);                    // Initialize wheelchair object
-Thread compass;                         // Thread for compass
-Thread velocity;                        // Thread for velosity
-Thread  ToFSafe;                        // Thread for safety stuff
-Thread emergencyButton;                 // Thread to check button state and reset device
+/**************************************************************************
+ *                          Thread Definitions                            *
+ **************************************************************************/
 
+Timer t;                                                        // Initialize time object t
+EventQueue queue;                                               // Class to organize threads
+Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT);    // Initialize wheelchair object
+Thread compass;                                                 // Thread for compass
+Thread velocity;                                                // Thread for velocity
+Thread ToFSafe;                                                 // Thread for safety stuff
+Thread emergencyButton;                                         // Thread to check button state and reset device
 
+/**************************************************************************
+ *                              MAIN CODE                                 *
+ **************************************************************************/
 int main(void)
-{   
-/*    nh.initNode();
+{  
+ 
+/*  nh.initNode();
     nh.advertise(chatter);
     nh.advertise(chatter2);
-    nh.subscribe(sub);*/
+    nh.subscribe(sub); */
+    
+    pc.printf("Before Starting\r\n");
     
-    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::velocity_thread);             // Sets up sampling frequency of the velocity thread
-    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::ToFSafe_thread);              // Sets up sampling frequency of the ToF safety thread
-    //queue.call_every(200, rosCom_thread);                                         // Sets up sampling frequency of the ROS com thread
-    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread);      // Sets up sampling frequency of the emergency button 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 velocity thread
+    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::ToFSafe_thread);          // Sets up sampling frequency of the ToF safety thread
+    //queue.call_every(200, rosCom_thread);                                     // Sets up sampling frequency of the ROS com thread
+    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread);  // Sets up sampling frequency of the emergency button thread
     
     t.reset();
-    //compass.start(callback(&queue, &EventQueue::dispatch_forever));               // Starts running the compass thread
-    velocity.start(callback(&queue, &EventQueue::dispatch_forever));                // Starts running the velocity thread
-    ToFSafe.start(callback(&queue, &EventQueue::dispatch_forever));                 // Starts running the ROS com thread
-    //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));               // Starts running the ROS com thread
-    emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever));         // Starts running the emergency button thread
+    //compass.start(callback(&queue, &EventQueue::dispatch_forever));           // Starts running the compass thread
+    velocity.start(callback(&queue, &EventQueue::dispatch_forever));            // Starts running the velocity thread
+    ToFSafe.start(callback(&queue, &EventQueue::dispatch_forever));             // Starts running the ROS com thread
+    //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));           // Starts running the ROS com thread
+    emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever));     // Starts running the emergency button thread
     
-        pc.printf("After starting\r\n");
+    pc.printf("After Starting\r\n");
 
     //added
- //   int emerg_button = e_button;
+    //int emerg_button = e_button;
     Watchdog dog;                                                           // Creates Watchdog object
     dog.Configure(watchdogLimit);                                           // Configures timeout for Watchdog
     pc.printf("Code initiated\n");
     int set = 0;
+    
     while(1) {
         if( pc.readable()) {
             set = 1;
@@ -153,7 +176,7 @@
         wait(process);
         
         t.stop();
-        //pc.printf("Time elapsed: %f seconds and iteration = %d\n", t.read(), iteration);
+        //pc.printf("Time elapsed: %f seconds, Iteration = %d\n", t.read(), iteration);
         dog.Service();                                                      // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog
 
     }