Updated with emergency button and watchdog code

Dependencies:   wheelchaircontrol4 Watchdog ros_lib_kinetic

Files at this revision

API Documentation at this revision

Comitter:
t1jain
Date:
Fri Jun 28 17:27:30 2019 +0000
Parent:
14:240bcc258bfe
Commit message:
Cleaned up code

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
wheelchaircontrol.lib Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Thu Jun 27 22:55:56 2019 +0000
+++ b/main.cpp	Fri Jun 28 17:27:30 2019 +0000
@@ -1,40 +1,36 @@
 #include "mbed.h"
 #include <ros.h>
-#include <nav_msgs/Odometry.h>      // Contains both the twist and pose
+#include <nav_msgs/Odometry.h>          // Contains both the twist and pose
 #include "Watchdog.h"
 #include "wheelchair.h"
 #include "rtos.h"
 
-QEI wheel (D10, D9, NC, 1200);        //Initializes right encoder
-DigitalIn pt3(D10, PullUp);          //Pull up resistors to read analog signals into digital signals
+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);
 
-double watchdogLimit = 0.1;        // Set timeout limit for watchdog timer in seconds
+double watchdogLimit = 0.1;             // Set timeout limit for watchdog timer in seconds
 int buttonCheck = 0;
 int iteration = 1;
 
-/*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
+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;
-//Timer testAccT;
 
-AnalogIn x(A0);                     //Initializes analog axis for the joystick
+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
-bool manual = false;                //Turns chair joystic to automatic and viceverza
+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
+bool manual = false;                    // Turns chair joystic to automatic and viceverza
 
-Serial pc(USBTX, USBRX, 57600);     //Serial Monitor
+Serial pc(USBTX, USBRX, 57600);         // Serial Monitor
 
-VL53L1X sensor1(PD_13, PD_12, PA_15);         //initializes ToF sensors
+VL53L1X sensor1(PD_13, PD_12, PA_15);   // Initializes ToF sensors
 VL53L1X sensor2(PD_13, PD_12, PC_7);
 VL53L1X sensor3(PD_13, PD_12, PB_5);
 VL53L1X sensor4(PD_13, PD_12, PE_11);
@@ -48,97 +44,88 @@
 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
+&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
+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 assistSafe;                  //thread for safety stuff
-Thread emergencyButton;             // Thread to check button state and reset device
+Thread compass;                         // Thread for compass
+Thread velocity;                        // Thread for velosity
+Thread assistSafe;                      // Thread for safety stuff
+Thread emergencyButton;                 // Thread to check button state and reset device
 
-int main(void)
-{   
-/*    nh.initNode();
-    nh.advertise(chatter);
-    nh.advertise(chatter2);
-    nh.subscribe(sub);*/
-    //testAccT.start();
+int main(void) {   
+
     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::assistSafe_thread); //Sets up sampling frequency of the velosity_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::assistSafe_thread);   // Sets up sampling frequency of the velosity_thread
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::emergencyButton_thread);      // Sets up sampling frequency of the emergencyButton_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
-    velocity.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
-    assistSafe.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity 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
     emergencyButton.start(callback(&queue, &EventQueue::dispatch_forever)); 
     
-    //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
+    //ros_com.start(callback(&queue, &EventQueue::dispatch_forever));       // Starts running the velosity thread
         pc.printf("After starting\r\n");
 
-    //added
- //   int emerg_button = e_button;
+    Watchdog dog;                                                           // Creates Watchdog object
+    dog.Configure(watchdogLimit);                                           // Configures timeout for Watchdog
+    pc.printf("Code initiated\n");
 
- Watchdog dog;                                                          // Creates Watchdog object
- dog.Configure(watchdogLimit);                                          // Configures timeout for Watchdog
- pc.printf("Code initiated\n");
-
- 
     int set = 0;
     while(iteration++) {
         t.start();
         
         if( pc.readable()) {
             set = 1;
-            char c = pc.getc();                                         //Read the instruction sent
+            char c = pc.getc();                                             // Read the instruction sent
             if( c == 'w') {
                 pc.printf("Moving forward\n");
-                smart.forward();                                        //Move forward
+                smart.forward();                                            // Move forward
             }
             else if( c == 'a') {
                 pc.printf("Turning left\n");               
-                smart.left();                                           //Turn left   
+                smart.left();                                               // Turn left   
             }
             else if( c == 'd') {
                 pc.printf("Turning right\n");
-                smart.right();                                          //Turn right
+                smart.right();                                              // Turn right
             }
             else if( c == 's') {
                 pc.printf("Moving back\n");
-                smart.backward();                                       //Turn backwards
+                smart.backward();                                           // Turn backwards
             }
             
             else if( c == 't') {                                        
                 smart.pid_twistA();
             } else if(c == 'v'){
                 smart.showOdom();
-            } else if(c == 'o') {                                       //Turns on chair
+            } else if(c == 'o') {                                           // Turns on chair
                 pc.printf("turning on\r\n");
                 on = 1;
                 wait(1);
                 on = 0;
-            } else if(c == 'f') {                                       //Turns off chair
+            } else if(c == 'f') {                                           // Turns off chair
                 pc.printf("turning off\r\n");
                 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
+            } else if( c == 'm' || manual) {                                // Turns wheelchair to joystick
                 pc.printf("turning on joystick\r\n");
                 manual = true;
                 t.reset();
                 while(manual) {
-                    smart.move(x,y);                                    //Reads from joystick and moves
+                    smart.move(x,y);                                        // Reads from joystick and moves
                     if( pc.readable()) {
                         char d = pc.getc();
-                        if( d == 'm') {                                 //Turns wheelchair from joystick into auto
+                        if( d == 'm') {                                     // Turns wheelchair from joystick into auto
                             pc.printf("turning off joystick\r\n");
                             manual = false;
                         }
@@ -147,19 +134,18 @@
             }
              else {
                     pc.printf("none \r\n");
-                    smart.stop();                                      //If nothing else is happening stop the chair
+                    smart.stop();                                           // If nothing else is happening stop the chair
             }
         }
         else {
             
-            smart.stop();                                              //If nothing else is happening stop the chair
+            smart.stop();                                                   // If nothing else is happening stop the chair
         }
 
-        wait(process);
-        
+        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
+        dog.Service();                                                      // Service the Watchdog so it does not cause a system reset - "Kicking"/"Feeding" the dog
     }
     
 }
--- a/wheelchaircontrol.lib	Thu Jun 27 22:55:56 2019 +0000
+++ b/wheelchaircontrol.lib	Fri Jun 28 17:27:30 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/Affordable-Smart-Wheelchair/code/wheelchaircontrol4/#052247c0f0e0
+https://os.mbed.com/teams/Affordable-Smart-Wheelchair/code/wheelchaircontrol4/#a6659c5a8109