Diff: main.cpp

Revision:

7:04f93e6b929f

Parent:

6:e9b1684a9c00

Child:

8:db780b392bae

--- a/main.cpp	Wed Aug 15 17:13:55 2018 +0000
+++ b/main.cpp	Wed Aug 29 16:53:41 2018 +0000
@@ -7,7 +7,6 @@
 DigitalOut down(D3);
 DigitalOut on(D12);
 DigitalOut off(D11);
-
 bool manual = false;
 
 Serial pc(USBTX, USBRX, 57600);
@@ -18,41 +17,44 @@
 //MPU9250 imu(D14, D15);
 Wheelchair smart(xDir,yDir, &pc, &t);
 Thread thread;
+Thread thread2;
 
 
 int main(void)
 {
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
+    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::distance_thread);
     t.reset();
     thread.start(callback(&queue, &EventQueue::dispatch_forever));
+    thread2.start(callback(&queue, &EventQueue::dispatch_forever));
+   // thread2.start(callback(&queue, &EventQueue::dispatch_forever));
     while(1) {
         if( pc.readable()) {
             char c = pc.getc();
-
             if( c == 'w') {
-                pc.printf("up \n");
+                //pc.printf("up \r\n");
                 smart.forward();
             }
 
             else if( c == 'a') {
-                //pc.printf("left \n");
+                //pc.printf("left \r\n");
                 smart.left();
             }
 
             else if( c == 'd') {
-                //pc.printf("right \n");
+                //pc.printf("right \r\n");
                 smart.right();
             }
 
             else if( c == 's') {
-                pc.printf("down \n");
+                //pc.printf("down \r\n");
                 smart.backward();
             }
 
             else if( c == 'r') {
                 smart.turn_right(90);
             }
-
+ 
             else if( c == 'l') {
                 smart.turn_left(90);
             }
@@ -60,27 +62,37 @@
             else if( c == 't') {
                 char buffer[256];
                 pc.printf ("Enter a long number: ");
-                fgets (buffer, 256, stdin);
-                int angle = atoi (buffer);
+                //fgets (buffer, 256, stdin);
+                int angle = 90;//atoi (buffer);
 
                 if(angle == 0) {
-                    pc.printf("invalid input try again\n");
+                    pc.printf("invalid input try again\r\n");
                 } else {
                     smart.pid_turn(angle);
                 }
 
             } else if(c == 'o') {
-                pc.printf("turning on\n");
+                pc.printf("turning on\r\n");
                 on = 1;
                 wait(1);
                 on = 0;
             } else if(c == 'f') {
-                pc.printf("turning off\n");
+                pc.printf("turning off\r\n");
                 off = 1;
                 wait(1);
                 off = 0;
+            } else if(c == 'u') {
+                double displacement = -1000;
+                if(displacement > 0)
+                {
+                    smart.pid_foward(displacement);
+                }
+                else if(displacement < 0)
+                {
+                    smart.pid_reverse(displacement);
+                }    
             } else if( c == 'm' || manual) {
-                pc.printf("turning on joystick\n");
+                pc.printf("turning on joystick\r\n");
                 manual = true;
                 t.reset();
                 while(manual) {
@@ -88,21 +100,20 @@
                     if( pc.readable()) {
                         char d = pc.getc();
                         if( d == 'm') {
-                            pc.printf("turning off joystick\n");
+                            pc.printf("turning off joystick\r\n");
                             manual = false;
                         }
                     }
-                }
+                }   
             }
-
             else {
-                pc.printf("none \n");
+                pc.printf("none \r\n");
                 smart.stop();
             }
         }
 
         else {
-            //        pc.printf("Nothing pressed \n");
+            //        pc.printf("Nothing pressed \r\n");
             smart.stop();
         }
         wait(process);