ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
Diff: PiControlThread.cpp
- Revision:
- 17:1184df616383
- Parent:
- 16:58ec2b891a25
- Child:
- 19:4fbffc755ed7
diff -r 58ec2b891a25 -r 1184df616383 PiControlThread.cpp --- a/PiControlThread.cpp Sun Mar 11 00:37:58 2018 +0000 +++ b/PiControlThread.cpp Sun Mar 11 01:26:23 2018 +0000 @@ -41,12 +41,12 @@ // steering gain float Ks = 0.15; - // distance gain float Kd = 10; // overall robot required speed int Setpoint; +Mutex mutexSetpoint; osThreadId PiControlId; @@ -125,7 +125,7 @@ // If distance increase then speed should increase. // If object is moving away from the the robot increase robot speed - cameraData_mutex.lock(); + /*cameraData_mutex.lock(); if(DistanceError > 0) { // Proportional controller @@ -142,7 +142,14 @@ setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); cameraData_mutex.unlock(); + */ + /********************Manual Setpoint and Steering**********************/ + mutexSetpoint.lock(); + setpointR = Setpoint + (Ks*SteeringError); + setpointL = Setpoint - (Ks*SteeringError); + mutexSetpoint.unlock(); + // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 560); setpointL = SaturateValue(setpointL, 560); @@ -162,7 +169,7 @@ motorDriver_R_reverse(U_right); } - // Set speed and direction of left motor + // Set speed and direction for left motor if (U_left >= 0) { motorDriver_L_forward(U_left);