Diff: main.cpp

Revision:

10:e5463c11e0a0

Parent:

8:381a4ec3fef8

--- a/main.cpp	Sun Jul 22 18:47:04 2018 +0000
+++ b/main.cpp	Mon Jul 23 20:17:37 2018 +0000
@@ -8,37 +8,37 @@
 DigitalOut up(D2);
 DigitalOut down(D3);
 
+bool manual = false;
+
 Serial pc(USBTX, USBRX, 57600);
 Timer t;
-Timer other;
 
-MPU9250 imu(D14, D15); 
+MPU9250 imu(D14, D15);
 //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
+
+    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
@@ -50,68 +50,68 @@
     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);
-    
+
     while(1) {
-   /*        if( pc.readable()) {
-               char c = pc.getc();
-               if( c == 'w') {
-                   pc.printf("up \n");
-                   smart.forward();
-                   }
+        if( pc.readable()) {
+            char c = pc.getc();
+
+            if( c == 'w') {
+                pc.printf("up \n");
+                smart.forward();
+            }
 
-               else if( c == 'd') {
-                   pc.printf("left \n");
-                   smart.left();
-                   }
+            else if( c == 'd') {
+                pc.printf("left \n");
+                smart.left();
+            }
 
-               else if( c == 'a') {
-                   pc.printf("right \n");
-                   smart.right();
-                   }
+            else if( c == 'a') {
+                pc.printf("right \n");
+                smart.right();
+            }
 
-               else if( c == 's') {
-                   pc.printf("down \n");
-                   smart.backward();
-                   }
+            else if( c == 's') {
+                pc.printf("down \n");
+                smart.backward();
+            }
 
-               else {
-                   pc.printf("none \n");
-                   smart.stop();
-                   if( c == 'o') {
-                      pc.printf("turning on");
-                      on = 0;
-                      wait(process);
-                      on = 1;
-                       }
+            else if( c == 'r') {
+                smart.turn_right();
+            }
 
-                   else if( c == 'k') {
-                       off = 0;
-                       wait(process);
-                       off = 1;
-                       }
+            else if( c == 'l') {
+                smart.turn_left();
+            }
 
-                   else if( c == 'u') {
-                       up = 0;
-                       wait(process);
-                       up = 1;
-                       }
+            else if( c == 'm' || manual) {
+                pc.printf("turning on joystick\n");
+                manual = true;
+                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 if( c == 'p') {
-                       down = 0;
-                       wait(process);
-                       down = 1;
-                       }
-                   }
-                  }
+            else {
+                pc.printf("none \n");
+                smart.stop();
+            }
+        }
 
-           else {
-              pc.printf("Nothing pressed \n");
-              smart.stop();
-                   }
-     */     
+        else {
+            //        pc.printf("Nothing pressed \n");
+            smart.stop();
+        }
+        /*
         smart.move(x,y);
         if( pc.readable()) {
             char c = pc.getc();
@@ -120,8 +120,8 @@
             }
             if( c == 'l') {
                 smart.turn_left();
-                }
-        }
+            }
+        }*/
         wait(process);
     }