Diff: main.cpp

Revision:

11:75f0f13ff6c1

Parent:

10:e5463c11e0a0

Child:

12:0e5a0571b497

diff -r e5463c11e0a0 -r 75f0f13ff6c1 main.cpp
--- a/main.cpp	Mon Jul 23 20:17:37 2018 +0000
+++ b/main.cpp	Thu Aug 16 16:42:45 2018 +0000
@@ -13,62 +13,27 @@
 Serial pc(USBTX, USBRX, 57600);
 Timer t;
 
-MPU9250 imu(D14, D15);
-//Wheelchair smart(xDir,yDir, &pc, &t);
+Wheelchair smart(xDir,yDir, &pc, &t);
+
 
 int main(void)
 {
-
-    uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x71\n\r");
-
-    if (whoami == 0x71) { // WHO_AM_I should always be 0x68
-        pc.printf("MPU9250 is online...\n\r");
-
-        wait(1);
-
-        imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
-        imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-        imu.initMPU9250();
-        imu.initAK8963(imu.magCalibration);
-        wait(2);
-
-    } else {
-        pc.printf("Could not connect to MPU9250: \n\r");
-        pc.printf("%#x \n",  whoami);
-
-        while(1) ; // Loop forever if communication doesn't happen
-    }
-
-    imu.getAres(); // Get accelerometer sensitivity
-    imu.getGres(); // Get gyro sensitivity
-    imu.getMres(); // Get magnetometer sensitivity
-    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
-    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
-    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
-    imu.magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
-    imu.magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
-    imu.magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
-
-
-    Wheelchair smart(xDir,yDir, &pc, &t);
-
+    pc.printf("hello\n");
     while(1) {
         if( pc.readable()) {
             char c = pc.getc();
-
+            pc.printf("hello\n");
             if( c == 'w') {
                 pc.printf("up \n");
                 smart.forward();
             }
 
-            else if( c == 'd') {
+            else if( c == 'a') {
                 pc.printf("left \n");
                 smart.left();
             }
 
-            else if( c == 'a') {
+            else if( c == 'd') {
                 pc.printf("right \n");
                 smart.right();
             }
@@ -79,16 +44,32 @@
             }
 
             else if( c == 'r') {
-                smart.turn_right();
+                smart.turn_right(90);
             }
 
             else if( c == 'l') {
-                smart.turn_left();
+                smart.turn_left(90);
             }
 
-            else if( c == 'm' || manual) {
+            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.turn(angle);
+                    }
+                    
+            } 
+            
+            else if( c == 'm') {
                 pc.printf("turning on joystick\n");
                 manual = true;
+                t.reset();
                 while(manual) {
                     smart.move(x,y);
                     if( pc.readable()) {
@@ -108,20 +89,8 @@
         }
 
         else {
-            //        pc.printf("Nothing pressed \n");
             smart.stop();
         }
-        /*
-        smart.move(x,y);
-        if( pc.readable()) {
-            char c = pc.getc();
-            if( c == 'r') {
-                smart.turn_right();
-            }
-            if( c == 'l') {
-                smart.turn_left();
-            }
-        }*/
         wait(process);
     }