PlayBack
Dependencies: TPixy-Interface
Fork of ObjectFollower by
Diff: PiControlThread.cpp
- Revision:
- 10:8919b1b76243
- Parent:
- 9:fe56b888985c
- Child:
- 11:9135e5bc2fcf
--- a/PiControlThread.cpp Fri Feb 23 20:58:34 2018 +0000 +++ b/PiControlThread.cpp Fri Mar 02 23:37:31 2018 +0000 @@ -21,9 +21,14 @@ #include "PiControlThread.h" #include "Drivers/PiController.h" +// setpoints extern int setpointR, setpointL; +extern int SteeringError, DistanceError; +// int velR, velL; + +// control signal int32_t U_right, U_left; sensors_t sensors; @@ -33,6 +38,15 @@ void PiControlThread(void const *); void PeriodicInterruptISR(void); +// steering gain +int Ks = 1; + +// distance gain +int Kd = 1; + +// overall robot required speed +int Setpoint; + osThreadId PiControlId; /******************************************************************************/ @@ -76,6 +90,10 @@ } + + + + /******************************************************************************* * @brief This is the PI controller thread. It reads several values from the * FPGA such as speed, time and other sensors data @@ -89,8 +107,6 @@ { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - //time_passed++; - // get incremental position and time from QEI DE0_read(&sensors); @@ -103,6 +119,27 @@ // maximum velocity at dPostition = 560 is vel = 703 velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; + /*********************Differential Start*******************************/ + // Inputs are Speed Setpoint and Steering Setpoint + // The Inputs for the Steering are specified in the CameraThread + // and set at the center. + // The distance between the object and the image should be set to 1 meter + // If distance decrease then speed should stop. + // If distance increase then speed should increase. + + // if object is moving away from the the robot increase robot speed + if(DistanceError > 0) + { + Setpoint = (Kd*DistanceError); + } + // if object is at the set distance limit or less then do not move. + else if(DistanceError <= 0) + { + Setpoint = 0; + } + + setpointR = Setpoint + (Ks*SteeringError); + setpointL = Setpoint - (Ks*SteeringError); U_right = PiControllerR(setpointR,sensors.dp_right); U_left = PiControllerL(setpointL,sensors.dp_left);