ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
Diff: PiControlThread.cpp
- Revision:
- 15:cf67f83d5409
- Parent:
- 14:5777377537a2
- Child:
- 16:58ec2b891a25
--- a/PiControlThread.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/PiControlThread.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -20,10 +20,11 @@ #include "Drivers/DE0_driver.h" #include "PiControlThread.h" #include "Drivers/PiController.h" +#include "ui.h" +#include "CameraThread.h" // setpoints -extern int setpointR, setpointL; -extern int SteeringError, DistanceError; + // int velR, velL; @@ -39,10 +40,10 @@ void PeriodicInterruptISR(void); // steering gain -int Ks = 2; +int Ks = 0.5; // distance gain -int Kd = 2; +int Kd = 4; // overall robot required speed int Setpoint; @@ -105,8 +106,9 @@ // Get incremental position and time from QEI DE0_read(&sensors); - SaturateValue(sensors.dp_right, 560); - SaturateValue(sensors.dp_left, 560); + // might not be needed + sensors.dp_right = SaturateValue(sensors.dp_right, 560); + sensors.dp_left = SaturateValue(sensors.dp_left, 560); // Maximum velocity at dPostition = 560 is vel = 703 velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; @@ -123,10 +125,12 @@ // If distance increase then speed should increase. // If object is moving away from the the robot increase robot speed + cameraData_mutex.lock(); if(DistanceError > 0) { // Proportional controller Setpoint = (Kd*DistanceError); + Setpoint = SaturateValue(Setpoint,560); } // If object is at the set distance limit or less then do not move. else if(DistanceError <= 0) @@ -137,6 +141,7 @@ // Decoupled drive setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); + cameraData_mutex.unlock(); // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 560);