ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 4:417e475239c7, committed 2018-02-09
- Comitter:
- asobhy
- Date:
- Fri Feb 09 18:37:11 2018 +0000
- Parent:
- 3:4def4ca68910
- Child:
- 5:b29220d29022
- Child:
- 6:e7ce340fe91e
- Commit message:
- beginning of lab 02/09/2018
Changed in this revision
--- a/PiControlThread.cpp Sat Feb 03 00:48:47 2018 +0000 +++ b/PiControlThread.cpp Fri Feb 09 18:37:11 2018 +0000 @@ -3,29 +3,17 @@ #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" +#include "PiController.h" -// global speed variable; extern int setpoint; -extern Serial pc; -extern Mutex setpoint_mutex; uint16_t ID, dTime; int dPosition; int vel; - -int32_t e, u, xState; - -bool saturationFlag; - -float Ki; -float Kp; +int32_t U; void PiControlThread(void const *); void PeriodicInterruptISR(void); -int SaturatingSubtract(int , int ); -int SaturatingAdd(int , int ); -int SaturateValue(int , int ); - osThreadId PiControlId; @@ -49,18 +37,16 @@ void PiControlThreadInit() { - DE0_init(); // initialize FPGA - motorDriver_init(); // initialize motorDriver + DE0_init(); // initialize FPGA + motorDriver_init(); // initialize motorDriver + PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables PiControlId = osThreadCreate(osThread(PiControlThread), NULL); // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate - - Kp = 2.2; - Ki = 0.01; - + } @@ -70,16 +56,6 @@ void PiControlThread(void const *argument) { - // initialization - saturationFlag = false; - int scale = 40; - xState = 0; - - int32_t xTemp; - int32_t uProportional; - int32_t uIntegral; - int32_t uS; - while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. @@ -91,30 +67,15 @@ // maximum velocity at dPostition = 560 is vel = 703 vel = (float)((6135.92 * dPosition) / dTime) ; - setpoint_mutex.lock(); - e = SaturatingSubtract(setpoint, dPosition); // e is the velocity error - setpoint_mutex.unlock(); - - xTemp = SaturatingAdd(xState, e); - - // the maximum value that 'u' can get to is 20 - // the maximum value that dPosition can get to 560 - // scaling factor is 560/20 = 28 - uProportional = (float)(Kp*e/scale); - uIntegral = (float)(Ki*xState/scale); + U = PiController(setpoint,dPosition); - uS = SaturatingAdd(uProportional, uIntegral); - - u = SaturateValue(uS, U_LIMIT); - if(u==uS) xState=xTemp; - - if (u >= 0) + if (U >= 0) { - motorDriver_forward(u); + motorDriver_forward(U); } - else if (u < 0) + else if (U < 0) { - motorDriver_reverse(u); + motorDriver_reverse(U); } } @@ -132,31 +93,3 @@ osSignalSet(PiControlId,0x1); } - -/*****************************************************************************/ -int SaturatingSubtract(int x, int y) -{ - int z; - z = x - y; // 32-bit overflow detection and saturating arithmetic - if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF; - else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000; - return z; -} - -/*****************************************************************************/ -int SaturatingAdd(int x, int y) -{ - int z; - z = x + y; // 32-bit overflow detection and saturating arithmetic - if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF; - else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000; - return z; -} - -/*****************************************************************************/ -int SaturateValue(int x, int Limit) -{ - if(x > Limit) return(Limit); // Impose maximum limit on x - else if(x < -Limit) return(-Limit); - else return(x); -}
--- a/PiControlThread.h Sat Feb 03 00:48:47 2018 +0000 +++ b/PiControlThread.h Fri Feb 09 18:37:11 2018 +0000 @@ -2,7 +2,6 @@ #ifndef PERIODIC_INT_H #define PERIODIC_INT_H -#define U_LIMIT 20 void PiControlThreadInit(void);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PiController.cpp Fri Feb 09 18:37:11 2018 +0000 @@ -0,0 +1,86 @@ +#include "mbed.h" +#include "PiController.h" + + + +// global speed variable; + +extern Mutex setpoint_mutex; + +float Ki; +float Kp; +int32_t e, u, xState, scale; + + +/*****************************************************************************/ +int SaturatingSubtract(int x, int y) +{ + int z; + z = x - y; // 32-bit overflow detection and saturating arithmetic + if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF; + else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000; + return z; +} + +/*****************************************************************************/ +int SaturatingAdd(int x, int y) +{ + int z; + z = x + y; // 32-bit overflow detection and saturating arithmetic + if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF; + else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000; + return z; +} + +/*****************************************************************************/ +int SaturateValue(int x, int Limit) +{ + if(x > Limit) return(Limit); // Impose maximum limit on x + else if(x < -Limit) return(-Limit); + else return(x); +} + + +/*****************************************************************************/ +void PiController_init(float Kp_given, float Ki_given) +{ + Kp = Kp_given; + Ki = Ki_given; + + // initialization + scale = 40; + xState = 0; + +} + + +/*****************************************************************************/ +uint32_t PiController(int setp, int dP) +{ + + int32_t xTemp; + int32_t uProportional; + int32_t uIntegral; + int32_t uS; + + setpoint_mutex.lock(); + e = SaturatingSubtract(setp, dP); // e is the velocity error + setpoint_mutex.unlock(); + + xTemp = SaturatingAdd(xState, e); + + // the maximum value that 'u' can get to is 20 + // the maximum value that dPosition can get to 560 + // scaling factor is 560/20 = 28 + + uProportional = (float)(Kp*e/scale); + uIntegral = (float)(Ki*xState/scale); + + uS = SaturatingAdd(uProportional, uIntegral); + + u = SaturateValue(uS, U_LIMIT); + if(u==uS) xState=xTemp; // if limit has not been reached then update xState + + return u; + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PiController.h Fri Feb 09 18:37:11 2018 +0000 @@ -0,0 +1,13 @@ +#ifndef PICONTROLLER_H +#define PICONTROLLER_H + +#define U_LIMIT 20 + +int SaturateValue(int , int ); +int SaturatingSubtract(int x, int y); +int SaturatingAdd(int x, int y); +void PiController_init(float,float); +uint32_t PiController(int,int); + + +#endif \ No newline at end of file
--- a/main.cpp Sat Feb 03 00:48:47 2018 +0000 +++ b/main.cpp Fri Feb 09 18:37:11 2018 +0000 @@ -28,7 +28,7 @@ while(1) { - consoleUI2(); + consoleUI(); Thread::wait(500); // Go to sleep for 500 ms } }
--- a/ui.cpp Sat Feb 03 00:48:47 2018 +0000 +++ b/ui.cpp Fri Feb 09 18:37:11 2018 +0000 @@ -60,6 +60,7 @@ /****************************************************************************** User interface ******************************************************************************/ +/* void consoleUI(void) { if (bluetooth.readable()) { @@ -113,12 +114,12 @@ bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, xState, dTime, Kp, Ki, vel, e); } - +*/ /****************************************************************************** User interface 2 ******************************************************************************/ -void consoleUI2(void) +void consoleUI(void) { if (bluetooth.readable()) { @@ -139,7 +140,7 @@ if ( setpoint < 506 ) { //setpoint = setpoint + SPEED_STEP; - setpoint = 200; + setpoint = 400; } setpoint_mutex.unlock(); @@ -154,7 +155,7 @@ setpoint_mutex.lock(); if (setpoint > -560) { - setpoint = -200; + setpoint = -400; //setpoint = setpoint - SPEED_STEP; } @@ -163,21 +164,21 @@ // display speed bluetooth.printf("\r\n %5d", setpoint); } - else if (x=='e') + else if (x=='i') { - Ki = Ki + 0.005; + Ki = Ki + 0.001; } - else if (x=='d') + else if (x=='k') { - Ki = Ki - 0.005; + Ki = Ki - 0.001; } - else if (x=='t') + else if (x=='o') { - Kp = Kp + 0.05; + Kp = Kp + 0.01; } - else if (x=='g') + else if (x=='l') { - Kp = Kp - 0.05; + Kp = Kp - 0.01; } // error wrong input
--- a/ui.h Sat Feb 03 00:48:47 2018 +0000 +++ b/ui.h Fri Feb 09 18:37:11 2018 +0000 @@ -3,17 +3,8 @@ #define SPEED_STEP 1 - - - void consoleUI(void); -void consoleUI2(void); void displayStartupMsg(void); void twoTerminalsTest(void); - - - - - #endif \ No newline at end of file