ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 1:3e9684e81312, committed 2018-02-03
- Comitter:
- asobhy
- Date:
- Sat Feb 03 00:05:08 2018 +0000
- Parent:
- 0:a355e511bc5d
- Child:
- 2:ca2a7430739b
- Commit message:
- setpoint represent velocity
Changed in this revision
--- a/Drivers/DE0_driver.cpp Thu Feb 01 03:58:00 2018 +0000 +++ b/Drivers/DE0_driver.cpp Sat Feb 03 00:05:08 2018 +0000 @@ -13,6 +13,19 @@ DigitalOut SpiReset(p14); // 0-1-0 tells the DE0 SPI interace that the next word sent is a control word +int SignExtend(int16_t x) +{ + + // if number is negative + if(x&0x00008000) { + // reserve the sign bit into the 32-bit number + x = x|0xFFFF0000; + } + + return x; +} + + // To reset all SPI peripheral on the FPGA: void ResetDE0(void){ IoReset=0; // Reset all DE0 peripherals @@ -67,7 +80,7 @@ */ -void DE0_read(uint16_t * id, uint16_t * dP, uint16_t * dT) +void DE0_read(uint16_t * id, int * dP, uint16_t * dT) { // To place SPI module into control mode, where the next word received by the // slave is interpreted as a control word. @@ -77,8 +90,8 @@ *id = (uint16_t)DE0.write(0x8002); // Read the two 16 bits registers from the FPGA - *dP = (uint16_t)DE0.write(DUMMY); // A SPI read only transaction occurs. - *dT = (uint16_t)DE0.write(DUMMY); // + *dP = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs. + *dT = DE0.write(DUMMY); // }
--- a/Drivers/DE0_driver.h Thu Feb 01 03:58:00 2018 +0000 +++ b/Drivers/DE0_driver.h Sat Feb 03 00:05:08 2018 +0000 @@ -5,7 +5,7 @@ void DE0_init(void); -void DE0_read(uint16_t *, uint16_t * , uint16_t *); +void DE0_read(uint16_t *, int * , uint16_t *);
--- a/Drivers/motor_driver.cpp Thu Feb 01 03:58:00 2018 +0000 +++ b/Drivers/motor_driver.cpp Sat Feb 03 00:05:08 2018 +0000 @@ -22,7 +22,7 @@ // motor is still uninitialized motor_status = MOTOR_UNINIT; - PwmR.period_ms(PERIOD); // This sets the PWM period in us + PwmR.period_us(PERIOD); // This sets the PWM period in us // DC Motor Control Pins DirR = 0; PwmR = 0;
--- a/Drivers/motor_driver.h Thu Feb 01 03:58:00 2018 +0000 +++ b/Drivers/motor_driver.h Sat Feb 03 00:05:08 2018 +0000 @@ -3,7 +3,7 @@ #define MOTOR_DRIVER_H //motor configuration -#define PERIOD 20000 // period in ms -> frequency = 50kHz -- audible frequency is between 20Hz - 20kHz +#define PERIOD 40 // period in us -> frequency = 25kHz -- audible frequency is between 20Hz - 20kHz typedef enum { MOTOR_UNINIT, MOTOR_INIT,
--- a/PiControlThread.cpp Thu Feb 01 03:58:00 2018 +0000 +++ b/PiControlThread.cpp Sat Feb 03 00:05:08 2018 +0000 @@ -2,19 +2,30 @@ #include "ui.h" #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" +#include "PiControlThread.h" // global speed variable; extern int setpoint; extern Serial pc; extern Mutex setpoint_mutex; -uint16_t ID, dPosition, dTime; -int e, u, xState; +uint16_t ID, dTime; +int dPosition; +int vel; -float Ki, Kp; +int32_t e, u, xState; + +bool saturationFlag; + +float Ki; +float Kp; void PiControlThread(void const *); void PeriodicInterruptISR(void); +int SaturatingSubtract(int , int ); +int SaturatingAdd(int , int ); +int SaturateValue(int , int ); + osThreadId PiControlId; @@ -45,7 +56,11 @@ // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: - PeriodicInt.attach(&PeriodicInterruptISR, 0.05); + PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate + + Kp = 2; + Ki = 0.01; + } @@ -53,23 +68,48 @@ * ******** Periodic Timer Interrupt Thread ******** *******************************************************************************/ void PiControlThread(void const *argument) -{ +{ + // initialization + saturationFlag = false; + int scale = 40; + while (true) { - osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled. // get incremental position and time from QEI DE0_read(&ID, &dPosition, &dTime); + SaturateValue(dPosition, 560); + + // maximum velocity at dPostition = 560 is vel = 703 + vel = (float)((6135.92 * dPosition) / dTime) ; setpoint_mutex.lock(); - e = setpoint-dPosition; // e is the velocity error + e = SaturatingSubtract(setpoint, vel); // e is the velocity error setpoint_mutex.unlock(); - xState = xState + e; // x is the Euler approximation to the integral of e. - u = Kp*e + Ki*xState; // u is the control signal - + // disable integration if u is saturated + if(saturationFlag) + { + xState = xState; + } + else + { + xState = SaturatingAdd(xState, e); // x is the Euler approximation to the integral of 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 + + u = (float)(Kp*e/scale) + (float)(Ki*xState/scale); // u is the control signal + + // if u is saturated set the flag to true + if( (u>= U_LIMIT) || (u<= -U_LIMIT) ) saturationFlag = true; + else saturationFlag = false; + + // saturate u at values greater than 20 and smaller than -20 + u = SaturateValue(u, U_LIMIT); if (u >= 0) { @@ -78,12 +118,10 @@ else if (u < 0) { motorDriver_reverse(u); - } - else - { - pc.printf("\r\nerror!!!"); - } + } + } + } /******************************************************************************* @@ -96,3 +134,32 @@ // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. 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 Thu Feb 01 03:58:00 2018 +0000 +++ b/PiControlThread.h Sat Feb 03 00:05:08 2018 +0000 @@ -2,6 +2,7 @@ #ifndef PERIODIC_INT_H #define PERIODIC_INT_H +#define U_LIMIT 20 void PiControlThreadInit(void);
--- a/main.cpp Thu Feb 01 03:58:00 2018 +0000 +++ b/main.cpp Sat Feb 03 00:05:08 2018 +0000 @@ -28,7 +28,7 @@ while(1) { - consoleUI(); + consoleUI2(); Thread::wait(500); // Go to sleep for 500 ms } }
--- a/ui.cpp Thu Feb 01 03:58:00 2018 +0000 +++ b/ui.cpp Sat Feb 03 00:05:08 2018 +0000 @@ -8,14 +8,21 @@ Mutex setpoint_mutex; // speed -int setpoint; +int setpoint = 0; // variable to store character recieved from terminal char x; -extern uint16_t dPosition, dTime; +extern int16_t dPosition, dTime; + +extern float Ki; +extern float Kp; -uint16_t position; +extern int vel; + +int16_t position; + +extern int32_t e; /****************************************************************************** A function to test blutooth communication @@ -40,63 +47,146 @@ ******************************************************************************/ void displayStartupMsg() { - pc.printf("\r\n************************************"); - pc.printf("\r\n**** DC Motor Control using PWM ****"); - pc.printf("\r\n************************************"); - pc.printf("\r\n-Enter r to reset the watchdog timer"); - pc.printf("\r\n-press w to increase motor speed"); - pc.printf("\r\n-press s to decrease motor speed"); + bluetooth.printf("\r\n************************************"); + bluetooth.printf("\r\n**** DC Motor Control using PWM ****"); + bluetooth.printf("\r\n************************************"); + bluetooth.printf("\r\n-Enter r to reset the watchdog timer"); + bluetooth.printf("\r\n-press w to increase motor speed"); + bluetooth.printf("\r\n-press s to decrease motor speed"); } /****************************************************************************** - Console interface + User interface ******************************************************************************/ void consoleUI(void) { - if (pc.readable()) { - x = pc.getc(); - + if (bluetooth.readable()) { + x = bluetooth.getc(); + // if input from console is the letter 'r' if(x == 'r') { // reset watchdog timer WatchdogReset(); - pc.printf("\r\nWatchdog has been reset"); + bluetooth.printf("\r\nWatchdog has been reset"); } // if w is pressed increase the speed // by incrementing u else if(x == 'w') { - // convert to integer - if(setpoint<=10000) { - setpoint_mutex.lock(); - setpoint = setpoint + SPEED_STEP; - setpoint_mutex.unlock(); + setpoint_mutex.lock(); + if ( setpoint < 560 ) + { + //setpoint = setpoint + SPEED_STEP; + setpoint = 100; } + setpoint_mutex.unlock(); + // display speed - pc.printf("\r\n %5d", setpoint); + bluetooth.printf("\r\n %5d", setpoint); } // if s is pressed decrease the speed // by decrementing u else if(x == 's') { - // convert to integer - if(setpoint>-10000) { - setpoint_mutex.lock(); - setpoint = setpoint - SPEED_STEP; - setpoint_mutex.unlock(); + + setpoint_mutex.lock(); + if (setpoint > -560) + { + setpoint = -100; + //setpoint = setpoint - SPEED_STEP; } + + setpoint_mutex.unlock(); + // display speed - pc.printf("\r\n %5d", setpoint); + bluetooth.printf("\r\n %5d", setpoint); } // error wrong input else { - pc.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog"); + bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog"); } } position += dPosition; - pc.printf("\r\nposition: %d, dPosition: %d, dTime: %d", position, dPosition, dTime); + bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e); } +/****************************************************************************** + User interface 2 +******************************************************************************/ + +void consoleUI2(void) +{ + + if (bluetooth.readable()) { + x = bluetooth.getc(); + + // if input from console is the letter 'r' + if(x == 'r') { + // reset watchdog timer + WatchdogReset(); + setpoint = 0; + bluetooth.printf("\r\nWatchdog has been reset"); + } + + // if w is pressed increase the speed + // by incrementing u + else if(x == 'w') { + setpoint_mutex.lock(); + if ( setpoint < 703 ) + { + //setpoint = setpoint + SPEED_STEP; + setpoint = 200; + } + setpoint_mutex.unlock(); + + // display speed + bluetooth.printf("\r\n %5d", setpoint); + } + + // if s is pressed decrease the speed + // by decrementing u + else if(x == 's') { + + setpoint_mutex.lock(); + if (setpoint > -703) + { + setpoint = -200; + //setpoint = setpoint - SPEED_STEP; + } + + setpoint_mutex.unlock(); + + // display speed + bluetooth.printf("\r\n %5d", setpoint); + } + else if (x=='e') + { + Ki = Ki + 0.005; + } + else if (x=='d') + { + Ki = Ki - 0.005; + } + else if (x=='t') + { + Kp = Kp + 0.05; + } + else if (x=='g') + { + Kp = Kp - 0.05; + } + + // error wrong input + else { + bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog"); + } + } + + position += dPosition; + //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e); + bluetooth.printf("\r\nKp: %f, Ki: %f, vel: %d, e: %d", Kp, Ki, vel, e); + +}
--- a/ui.h Thu Feb 01 03:58:00 2018 +0000 +++ b/ui.h Sat Feb 03 00:05:08 2018 +0000 @@ -1,12 +1,13 @@ #ifndef UI_H #define UI_H -#define SPEED_STEP 100 +#define SPEED_STEP 1 void consoleUI(void); +void consoleUI2(void); void displayStartupMsg(void); void twoTerminalsTest(void);