Diff: wheelchair.cpp

Revision:

42:62c49ad06707

Parent:

41:1a687bcf4b0a

Child:

43:9858a580b3d7

--- a/wheelchair.cpp	Thu Jul 11 20:58:26 2019 +0000
+++ b/wheelchair.cpp	Wed Aug 07 19:01:17 2019 +0000
@@ -113,7 +113,7 @@
     outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
     outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
 
-    for(int i = 0; i < 2; i++) {                             // Reads from the ToF Sensors
+    for(int i = 0; i < 2; i++) {        // Reads from the ToF Sensors
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
     }
 
@@ -155,7 +155,7 @@
     outlierToF[2] = LBTStats.mean() + 2*LBTStats.stdev();
     outlierToF[3] = RBTStats.mean() + 2*RBTStats.stdev();
 
-    for(int i = 2; i < 4; i++) {                             // Reads from the ToF Sensors
+    for(int i = 2; i < 4; i++) {        // Reads from the ToF Sensors
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
     }
 
@@ -178,6 +178,7 @@
         if(x->read() > def) {
             x->write(def);
             backwardSafety = 1;          // You cannot move backwards
+            // Stop moving backwards
         }
     }
 
@@ -192,10 +193,10 @@
     ////////////////////////////////////////////////////////////////////////////
 
     /*Side Tof begin*/
-    int sensor3 = ToFV[2]; //front left
-    int sensor6 = ToFV[5]; //front right
-    int sensor9 = ToFV[8]; //back
-    int sensor12 = ToFV[11]; //back
+    int sensor3 = ToFV[2];      //front left
+    int sensor6 = ToFV[5];      //front right
+    int sensor9 = ToFV[8];      //back
+    int sensor12 = ToFV[11];    //back
     
     double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU
     double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads
@@ -298,7 +299,7 @@
     /* Initializes IMU Library */
     out = pc;                                                                           // "out" is called for serial monitor
     out->printf("on\r\n");
-    imu = new IMUWheelchair(pc, time);
+    imu = new BNO080Wheelchair (pc, D2, D4, D13, D15, 0x4b, 100000);                    // THIS NEEDS TO BE REVISED SO THAT PIN NAMES ARE CALLED
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
     wheelS = qeiS;                                                                      // "wheel" is called for encoder