Diff: main.cpp

Revision:

8:db780b392bae

Parent:

7:04f93e6b929f

Child:

9:1081ebfe3db0

diff -r 04f93e6b929f -r db780b392bae main.cpp
--- a/main.cpp	Wed Aug 29 16:53:41 2018 +0000
+++ b/main.cpp	Fri Aug 31 20:01:55 2018 +0000
@@ -1,5 +1,13 @@
 #include "wheelchair.h"
 
+QEI wheel(D0, D1, NC, 1200);
+DigitalIn pt3(D1, PullUp);
+DigitalIn pt4(D0, PullUp);
+
+/*QEI wheel2 (D3, D6, NC, 1200);
+DigitalIn pt1(D3, PullUp);
+DigitalIn pt2(D6, PullUp);*/
+
 AnalogIn x(A0);
 AnalogIn y(A1);
 
@@ -15,27 +23,23 @@
 EventQueue queue;
 
 //MPU9250 imu(D14, D15);
-Wheelchair smart(xDir,yDir, &pc, &t);
+Wheelchair smart(xDir,yDir, &pc, &t, &wheel);
 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 \r\n");
                 smart.forward();
+                pc.printf("%f\r\n", wheel.getDistance(53.975));
             }
-
             else if( c == 'a') {
                 //pc.printf("left \r\n");
                 smart.left();
@@ -49,6 +53,7 @@
             else if( c == 's') {
                 //pc.printf("down \r\n");
                 smart.backward();
+               // wheel.reset();
             }
 
             else if( c == 'r') {
@@ -60,11 +65,20 @@
             }
 
             else if( c == 't') {
+
                 char buffer[256];
                 pc.printf ("Enter a long number: ");
+                char d = pc.getc();
+                int angle = 0;
                 //fgets (buffer, 256, stdin);
-                int angle = 90;//atoi (buffer);
-
+                if(d == 'r')
+                {
+                     angle = 90;//atoi (buffer);
+                }                
+                if(d == 'l')
+                {
+                     angle = -90;//atoi (buffer);
+                }
                 if(angle == 0) {
                     pc.printf("invalid input try again\r\n");
                 } else {
@@ -81,11 +95,30 @@
                 off = 1;
                 wait(1);
                 off = 0;
+            } else if(c == 'k'){
+                pc.printf("kitchen\r\n");
+                smart.kitchen();
+            } else if(c == 'e'){
+                pc.printf("desk\r\n");
+                smart.desk();
+            } else if(c == 'x'){
+                pc.printf("desk to kitchen\r\n");
+                smart.desk_to_kitchen();                
             } else if(c == 'u') {
-                double displacement = -1000;
+                pc.printf ("Enter a long number: ");
+                char d = pc.getc();
+                double displacement = 0;
+                if(d == '1')
+                    displacement = 5461;
+                if(d == '2')
+                    displacement = 3658;
+                if(d == '3')
+                    displacement = 305;
+                if(d == '4')
+                    displacement = 1000;
                 if(displacement > 0)
                 {
-                    smart.pid_foward(displacement);
+                    smart.pid_forward(displacement);
                 }
                 else if(displacement < 0)
                 {
@@ -96,7 +129,7 @@
                 manual = true;
                 t.reset();
                 while(manual) {
-                    smart.move(x,y);
+                    smart.move(x,y); 
                     if( pc.readable()) {
                         char d = pc.getc();
                         if( d == 'm') {