ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 7:73fd05fe556a, committed 2018-02-15
- Comitter:
- asobhy
- Date:
- Thu Feb 15 01:15:26 2018 +0000
- Parent:
- 6:e7ce340fe91e
- Child:
- 8:a0890fa79084
- Commit message:
- Added comments to several files and organized the code a bit
Changed in this revision
--- a/Drivers/DE0_driver.cpp Sat Feb 10 19:35:21 2018 +0000 +++ b/Drivers/DE0_driver.cpp Thu Feb 15 01:15:26 2018 +0000 @@ -15,13 +15,11 @@ 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; } @@ -43,6 +41,14 @@ SpiReset=0; } + +/******************************************************************************* +* @brief This is the initialization function for the DE0 FPGA - communication + between the FPGA and MCU is done through SPI. The function + initializes the SPI channel to a bit rate of 500kbps +* @param none +* @return none +*******************************************************************************/ void DE0_init(void) { DE0.format(16,1); // Define SPI format: 16-bit words, mode 1 protocol. @@ -50,8 +56,7 @@ ResetDE0(); } -/* - +/******************************************************************************* Before an SPI data transaction with the DE0 FPGA can occur, a control word must first be written to the slave that specifies: @@ -62,24 +67,27 @@ A control word need only be written once – if the same transactions are repeated. Subsequent transactions use the protocol specified by the last control word that - was written. To change the transaction protocol a new control needs to be - written. To write a new control word, the SpiReset input must first be set then - cleared followed by a SPI write of a 16-bit word that is interpreted as the - control word by the slave. +was written. To change the transaction protocol a new control needs to be +written. To write a new control word, the SpiReset input must first be set then +cleared followed by a SPI write of a 16-bit word that is interpreted as the +control word by the slave. - The 16-bit control word format appears below. +The 16-bit control word format appears below. 1 7bits 8bits rd | address offset | number of words - When RD = 1, no write operation occurs within the slave. Data on the MOSI is ignored by the slave. When RD = 0, a simultaneous read + write of the slave occurs. - - */ +*******************************************************************************/ +/******************************************************************************* +* @brief This function reads the output from the FPGA through the SPI channel +* @param none +* @return none +*******************************************************************************/ 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 @@ -93,7 +101,4 @@ *dP = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs. *dT = DE0.write(DUMMY); // } - - - \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Drivers/PiController.cpp Thu Feb 15 01:15:26 2018 +0000 @@ -0,0 +1,88 @@ +#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; + +} + + +/******************************************************************************* +* @brief PI Controller function +* @param setp is the setpoint we would like the system to get to +* @param dP is the current speed of the system +* @return u is the control signal out of the controller to make the system +* reach the desired setpoint +*******************************************************************************/ +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 + // scaling factor used is 40 + + 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; + +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Drivers/PiController.h Thu Feb 15 01:15:26 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/ExternalInterruptThread.cpp Sat Feb 10 19:35:21 2018 +0000 +++ b/ExternalInterruptThread.cpp Thu Feb 15 01:15:26 2018 +0000 @@ -12,14 +12,23 @@ DigitalOut led2(LED2); +/******************************************************************************* +* @brief This is the initialization function for ExternalInterruptthread +* @param none +* @return none +*******************************************************************************/ void ExternalInterruptThreadInit() { Bumper.rise(&ExtInterruptISR); // Attach the address of the interrupt handler to the rising edge of Bumper ExtInterruptId = osThreadCreate(osThread(ExtInterruptThread), NULL); } - -// ******** External Interrupt Thread ******** + +/******************************************************************************* +* @brief ******** External Interrupt Thread ******** +* @param none +* @return none +*******************************************************************************/ void ExtInterruptThread(void const *argument) { while (true) { @@ -29,7 +38,11 @@ } -// ******** External Interrupt Handler ******** +/******************************************************************************* +* @brief ******** External Interrupt ISR ******** +* @param none +* @return none +*******************************************************************************/ void ExtInterruptISR(void) { osSignalSet(ExtInterruptId,0x01); // Send signal to the thread with ID, ExtInterruptId, i.e., ExtInterruptThread.
--- a/PiControlThread.cpp Sat Feb 10 19:35:21 2018 +0000 +++ b/PiControlThread.cpp Thu Feb 15 01:15:26 2018 +0000 @@ -3,7 +3,7 @@ #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" -#include "PiController.h" +#include "Drivers/PiController.h" extern int setpoint; @@ -34,7 +34,13 @@ DigitalOut led3(LED3); - +/******************************************************************************* +* @brief function that creates a thread for the PI controller. It initializes +* the PI controller's gains and initializes the DC Motor. It also +* initializes the PIControllerThread runs at 50ms period +* @param none +* @return none +*******************************************************************************/ void PiControlThreadInit() { DE0_init(); // initialize FPGA @@ -52,7 +58,10 @@ /******************************************************************************* -* ******** Periodic Timer Interrupt Thread ******** +* @brief This is the PI controller thread. It reads several values from the +* FPGA such as speed, time and other sensors data +* @param none +* @return none *******************************************************************************/ void PiControlThread(void const *argument) { @@ -77,16 +86,17 @@ else if (U < 0) { motorDriver_reverse(U); - } + } } } /******************************************************************************* -* the interrupt below occures every 250ms as setup in the main function during -* initialization -* ******** Period Timer Interrupt Handler ******** +* @brief The ISR below signals the PIControllerThread. it is setup to run +* every 50ms +* @param none +* @return none *******************************************************************************/ void PeriodicInterruptISR(void) {
--- a/PiController.cpp Sat Feb 10 19:35:21 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,86 +0,0 @@ -#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
--- a/PiController.h Sat Feb 10 19:35:21 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,13 +0,0 @@ -#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/mbed-os.lib Sat Feb 10 19:35:21 2018 +0000 +++ b/mbed-os.lib Thu Feb 15 01:15:26 2018 +0000 @@ -1,1 +1,1 @@ -https://github.com/ARMmbed/mbed-os/#96d9a00d0a1d25095b330095fa81c40f7741777c +https://github.com/ARMmbed/mbed-os/#569159b784f70feaa32ce226aaca896fb83452f7