Diff: main.cpp

Revision:

14:240bcc258bfe

Parent:

13:1d0ed92043af

Child:

15:573dc6e9a1c6

--- a/main.cpp	Thu Jun 27 18:33:05 2019 +0000
+++ b/main.cpp	Thu Jun 27 22:55:56 2019 +0000
@@ -9,8 +9,9 @@
 DigitalIn pt3(D10, PullUp);          //Pull up resistors to read analog signals into digital signals
 DigitalIn pt4(D9, PullUp);
 
-double watchdogLimit = 1.00;        // Set timeout limit for watchdog timer in milliseconds
+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
@@ -65,7 +66,7 @@
     nh.advertise(chatter2);
     nh.subscribe(sub);*/
     //testAccT.start();
-    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::assistSafe_thread); //Sets up sampling frequency of the velosity_thread
@@ -82,29 +83,34 @@
 
     //added
  //   int emerg_button = e_button;
-  
+
  Watchdog dog;                                                          // Creates Watchdog object
  dog.Configure(watchdogLimit);                                          // Configures timeout for Watchdog
- pc.printf("Code initiated/reset");
- 
+ pc.printf("Code initiated\n");
+
  
     int set = 0;
-    while(1) {
+    while(iteration++) {
+        t.start();
+        
         if( pc.readable()) {
             set = 1;
             char c = pc.getc();                                         //Read the instruction sent
             if( c == 'w') {
-                smart.forward();                                        //Move foward
-
+                pc.printf("Moving forward\n");
+                smart.forward();                                        //Move forward
             }
             else if( c == 'a') {
-                smart.left();                                           //Turn left
+                pc.printf("Turning left\n");               
+                smart.left();                                           //Turn left   
             }
             else if( c == 'd') {
+                pc.printf("Turning right\n");
                 smart.right();                                          //Turn right
             }
             else if( c == 's') {
-                smart.backward();                                       //Turn rackwards
+                pc.printf("Moving back\n");
+                smart.backward();                                       //Turn backwards
             }
             
             else if( c == 't') {                                        
@@ -150,7 +156,11 @@
         }
 
         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
     }
+    
 }