Fully integrated ToF/IMU codes

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Revision:
0:7e6b349182bc
Child:
3:ef063fd4234e
Child:
5:90bf5f0d86e9
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 09 16:37:08 2018 +0000
@@ -0,0 +1,102 @@
+#include "wheelchair.h"
+
+AnalogIn x(A0);
+AnalogIn y(A1);
+
+DigitalOut off(D0);
+DigitalOut on(D1);
+DigitalOut up(D2);
+DigitalOut down(D3);
+
+bool manual = false;
+
+Serial pc(USBTX, USBRX, 57600);
+Timer t;
+EventQueue queue;
+
+//MPU9250 imu(D14, D15);
+Wheelchair smart(xDir,yDir, &pc, &t);
+Thread thread;
+
+int main(void)
+{
+    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
+    t.reset();
+    thread.start(callback(&queue, &EventQueue::dispatch_forever));
+    while(1) {
+        if( pc.readable()) {
+            char c = pc.getc();
+
+            if( c == 'w') {
+                pc.printf("up \n");
+                smart.forward();
+            }
+
+            else if( c == 'a') {
+                //pc.printf("left \n");
+                smart.left();
+            }
+
+            else if( c == 'd') {
+                //pc.printf("right \n");
+                smart.right();
+            }
+
+            else if( c == 's') {
+                pc.printf("down \n");
+                smart.backward();
+            }
+
+            else if( c == 'r') {
+                smart.turn_right(90);
+            }
+
+            else if( c == 'l') {
+                smart.turn_left(90);
+            }
+
+            else if( c == 't') {
+                char buffer[256];
+                pc.printf ("Enter a long number: ");
+                fgets (buffer, 256, stdin);
+                int angle = atoi (buffer);
+
+                if(angle == 0) {
+                    pc.printf("invalid input try again\n");
+                } else {
+                    smart.pid_turn(angle);
+                }
+
+            }
+
+            else if( c == 'm' || manual) {
+                pc.printf("turning on joystick\n");
+                manual = true;
+                t.reset();
+                while(manual) {
+                    smart.move(x,y);
+                    if( pc.readable()) {
+                        char d = pc.getc();
+                        if( d == 'm') {
+                            pc.printf("turning off joystick\n");
+                            manual = false;
+                        }
+                    }
+                }
+            }
+
+            else {
+                pc.printf("none \n");
+                smart.stop();
+            }
+        }
+
+        else {
+            //        pc.printf("Nothing pressed \n");
+            smart.stop();
+        }
+        wait(process);
+    }
+
+}
+