![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
Diff: PiControlThread.cpp
- Revision:
- 9:fe56b888985c
- Parent:
- 8:a0890fa79084
- Child:
- 10:8919b1b76243
diff -r a0890fa79084 -r fe56b888985c PiControlThread.cpp --- a/PiControlThread.cpp Mon Feb 19 17:42:33 2018 +0000 +++ b/PiControlThread.cpp Fri Feb 23 20:58:34 2018 +0000 @@ -1,9 +1,9 @@ /******************************************************************************/ // ECE4333 -// LAB Partner 1: Ahmed Sobhy - ID: 3594449 -// LAB Partner 2: Brandon Kingman - ID: 3470444 -// Project: Autonomous Robot Design -// Instructor: Prof. Chris Diduch +// LAB Partner 1: Ahmed Sobhy - ID: 3594449 +// LAB Partner 2: Brandon Kingman - ID: 3470444 +// Project: Autonomous Robot Design +// Instructor: Prof. Chris Diduch /******************************************************************************/ // filename: PiControlThread.cpp // file content description: @@ -15,17 +15,18 @@ #include "mbed.h" #include "ui.h" -#include "Drivers/motor_driver.h" +#include "Drivers/motor_driver_r.h" +#include "Drivers/motor_driver_l.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" #include "Drivers/PiController.h" -extern int setpoint; +extern int setpointR, setpointL; -uint16_t ID, dTime; -int dPosition; -int vel; -int32_t U; +int velR, velL; +int32_t U_right, U_left; + +sensors_t sensors; int time_passed = 0; @@ -61,7 +62,8 @@ void PiControlThreadInit() { DE0_init(); // initialize FPGA - motorDriver_init(); // initialize motorDriver + motorDriver_R_init(); // initialize motorDriver + motorDriver_L_init(); // initialize motorDriver // Kp,Ki PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables @@ -87,24 +89,42 @@ { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - time_passed++; + //time_passed++; // get incremental position and time from QEI - DE0_read(&ID, &dPosition, &dTime); - SaturateValue(dPosition, 560); + DE0_read(&sensors); + + SaturateValue(sensors.dp_right, 560); + SaturateValue(sensors.dp_left, 560); + + // maximum velocity at dPostition = 560 is vel = 703 + velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; // maximum velocity at dPostition = 560 is vel = 703 - vel = (float)((6135.92 * dPosition) / dTime) ; + velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; + - U = PiController(setpoint,dPosition); + U_right = PiControllerR(setpointR,sensors.dp_right); + U_left = PiControllerL(setpointL,sensors.dp_left); - if (U >= 0) + // set speed and direction for right motor + if (U_right >= 0) { - motorDriver_forward(U); + motorDriver_R_forward(U_right); + } + else if (U_right < 0) + { + motorDriver_R_reverse(U_right); } - else if (U < 0) + + // set speed and direction of left motor + if (U_left >= 0) { - motorDriver_reverse(U); + motorDriver_L_forward(U_left); + } + else if (U_left < 0) + { + motorDriver_L_reverse(U_left); } }