Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
Diff: wheelchair.cpp
- Revision:
- 26:c842070aa0b8
- Parent:
- 21:3489cffad196
- Child:
- 27:302d03ee1ae0
diff -r fb99cce6b9b5 -r c842070aa0b8 wheelchair.cpp --- a/wheelchair.cpp Fri Apr 19 23:06:53 2019 +0000 +++ b/wheelchair.cpp Sat May 25 03:32:36 2019 +0000 @@ -37,18 +37,18 @@ void Wheelchair::assistSafe_thread() { - int ToFV[12]; + /* int ToFV[12]; for(int i = 0; i < 9; 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) { @@ -59,7 +59,19 @@ forwardSafety = 1; } } - else + 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) + { + //out->printf("i am in danger\r\n"); + if(x->read() > def) + { + x->write(def); + forwardSafety = 1; + } + } + else*/ forwardSafety = 0; } @@ -85,7 +97,7 @@ for(int i = 0; i < 12; i++) // initializes the ToF Sensors { - (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); + //(*(ToF+i))->initReading(0x31+((0x02)*i), 50000); } out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor @@ -374,14 +386,14 @@ { /* Initializes variables as default */ double temporV = def; - double temporS = def; + double temporS = def+offset; vDesiredS = 0; x->write(def); - y->write(def); + y->write(def+offset); 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); @@ -393,7 +405,7 @@ while(1) { - linearV = .7; + //linearV = smart.linearV; test1 = linearV*100; vel = curr_vel; vDesired = linearV*100;