Updated references from MPU6050 to BNO080
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 42:62c49ad06707
- Parent:
- 41:1a687bcf4b0a
diff -r 1a687bcf4b0a -r 62c49ad06707 wheelchair.cpp --- 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