added code for PCB on and off, added pointers to e_button and pwm pins
Dependencies: QEI2 chair_BNO055 PID VL53L1X_Filter
Dependents: wheelchairControlSumer2019
Diff: wheelchair.cpp
- Revision:
- 26:662693bd7f31
- Parent:
- 21:3489cffad196
- Child:
- 27:0b1a837f123c
diff -r fb99cce6b9b5 -r 662693bd7f31 wheelchair.cpp --- a/wheelchair.cpp Fri Apr 19 23:06:53 2019 +0000 +++ b/wheelchair.cpp Thu Jun 27 16:32:12 2019 +0000 @@ -38,17 +38,29 @@ void Wheelchair::assistSafe_thread() { int ToFV[12]; - for(int i = 0; i < 9; i++) // reads from the ToF Sensors + for(int i = 0; i < 6; i++) // reads from the ToF Sensors { ToFV[i] = (*(ToF+i))->readFromOneSensor(); //out->printf("%d ", ToFV[i]); } - out->printf("\r\n"); - int sensor1 = ToFV[2]; + //out->printf("\r\n"); + int sensor1 = ToFV[1]; int sensor4 = ToFV[5]; - out->printf("%d, %d\r\n", sensor1, sensor4); - if(((2 * maxDeceleration*sensor1 < curr_vel*curr_vel*1000*1000 || - 2 * maxDeceleration*sensor4 < curr_vel*curr_vel*1000*1000) && + //out->printf("%d, %d\r\n", sensor1, sensor4); + /*if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || + 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && + (sensor1 < 1500 || sensor4 < 1500)) || + 550 > sensor1 || 550 > sensor4) + { + //out->printf("i am in danger\r\n"); + if(x->read() > def) + { + x->write(def); + forwardSafety = 1; + } + } + else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || + 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && (sensor1 < 1500 || sensor4 < 1500)) || 550 > sensor1 || 550 > sensor4) { @@ -61,7 +73,8 @@ } else forwardSafety = 0; - + } + */ } /* Constructor for Wheelchair class */ @@ -75,10 +88,11 @@ x = new PwmOut(xPin); y = new PwmOut(yPin); /* Initializes IMU Library */ + out = pc; // "out" is called for serial monitor + out->printf("on\r\n"); imu = new chair_BNO055(pc, time); Wheelchair::stop(); // Wheelchair is initially stationary imu->setup(); // turns on the IMU - out = pc; // "out" is called for serial monitor wheelS = qeiS; // "wheel" is called for encoder wheel = qei; ToF = ToFT; // passes pointer with addresses of ToF sensors @@ -110,11 +124,13 @@ /* Automatic mode: move forward and update x,y coordinate sent to chair */ void Wheelchair::forward() { + //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS); if(forwardSafety == 0) { x->write(high); y->write(def+offset); } + out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975)); } /* Automatic mode: move in reverse and update x,y coordinate sent to chair */ @@ -374,14 +390,14 @@ { /* Initializes variables as default */ double temporV = def; - double temporS = def; + double temporS = def+offset; vDesiredS = 0; x->write(def); y->write(def); wheel->reset(); // Resets the encoders /* Sets the constants for P and D */ PIDVelosity.SetTunings(.0005,0, 0.00); - PIDSlaveV.SetTunings(.01,0.000001, 0.000001); + PIDSlaveV.SetTunings(.005,0.000001, 0.000001); /* Limits to the range specified */ PIDVelosity.SetOutputLimits(-.005, .005);