for ros
Dependencies: QEI chair_BNO055 pid ros_lib_kinetic
Dependents: wheelchaircontrolrealtimeROS
Fork of wheelchaircontrol by
Diff: wheelchair.cpp
- Revision:
- 16:53d912c86c7f
- Parent:
- 14:9caca9fde9b0
- Child:
- 17:93a636b16b9e
diff -r ddd61f6dc5ab -r 53d912c86c7f wheelchair.cpp --- a/wheelchair.cpp Mon Aug 13 21:33:09 2018 +0000 +++ b/wheelchair.cpp Fri Aug 17 20:46:53 2018 +0000 @@ -5,22 +5,10 @@ //volatile double curr_yaw; double curr_yaw; double Setpoint, Output; -PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT); +//PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT); void Wheelchair::compass_thread() { - - //north = lowPass(imu->angle_north()); - //Input = (double)curr_yaw; curr_yaw = imu->yaw(); - north = boxcar(imu->angle_north()); - //out->printf("curr_yaw %f\n", curr_yaw); - //out->printf("%f\n", curr_yaw); - //out->printf("%f gyroz\n",imu->gyro_z()); - //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw); - - //out->printf("Yaw is %f\n", imu->yaw()); - //out->printf("north is %f\n", imu->angle_north()); - } Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time ) @@ -101,13 +89,14 @@ overturn = true; } - myPID.SetOutputLimits(low, def); + myPID.SetOutputLimits(0, def-low); myPID.SetControllerDirection(REVERSE); - out->printf("setpoint %f curr_yaw %f input %f output %i %i same address\n", Setpoint, curr_yaw, *(myPID.myInput), *(myPID.myOutput), (myPID.myInput==&curr_yaw), (myPID.myOutput == &Output) ); - out->printf("addresses %i %i\n", myPID.myInput, &curr_yaw); - while(myPID.Compute()) { - y->write(Output); + + while(Setpoint <= curr_yaw){ + myPID.Compute(); + y->write(Output+low); out->printf("curr_yaw %f\n", curr_yaw); + wait(process); } Wheelchair::stop(); out->printf("done \n");