ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@0:a355e511bc5d, 2018-02-01 (annotated)

Committer:
asobhy
Date:
Thu Feb 01 03:58:00 2018 +0000
Revision:
0:a355e511bc5d
Child:
1:3e9684e81312
before testing the QEI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 0:a355e511bc5d 1 #include "mbed.h"
asobhy 0:a355e511bc5d 2 #include "ui.h"
asobhy 0:a355e511bc5d 3 #include "Drivers/motor_driver.h"
asobhy 0:a355e511bc5d 4 #include "Drivers/DE0_driver.h"
asobhy 0:a355e511bc5d 5
asobhy 0:a355e511bc5d 6 // global speed variable;
asobhy 0:a355e511bc5d 7 extern int setpoint;
asobhy 0:a355e511bc5d 8 extern Serial pc;
asobhy 0:a355e511bc5d 9 extern Mutex setpoint_mutex;
asobhy 0:a355e511bc5d 10
asobhy 0:a355e511bc5d 11 uint16_t ID, dPosition, dTime;
asobhy 0:a355e511bc5d 12 int e, u, xState;
asobhy 0:a355e511bc5d 13
asobhy 0:a355e511bc5d 14 float Ki, Kp;
asobhy 0:a355e511bc5d 15
asobhy 0:a355e511bc5d 16 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 17 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 18
asobhy 0:a355e511bc5d 19 osThreadId PiControlId;
asobhy 0:a355e511bc5d 20
asobhy 0:a355e511bc5d 21 /******************************************************************************/
asobhy 0:a355e511bc5d 22 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 23 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 24 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 25 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 26 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 27 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 28 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 29 /******************************************************************************/
asobhy 0:a355e511bc5d 30
asobhy 0:a355e511bc5d 31 // Declare PeriodicInterruptThread as a thread/process
asobhy 0:a355e511bc5d 32 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 33
asobhy 0:a355e511bc5d 34 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 35
asobhy 0:a355e511bc5d 36 DigitalOut led3(LED3);
asobhy 0:a355e511bc5d 37
asobhy 0:a355e511bc5d 38
asobhy 0:a355e511bc5d 39 void PiControlThreadInit()
asobhy 0:a355e511bc5d 40 {
asobhy 0:a355e511bc5d 41 DE0_init(); // initialize FPGA
asobhy 0:a355e511bc5d 42 motorDriver_init(); // initialize motorDriver
asobhy 0:a355e511bc5d 43
asobhy 0:a355e511bc5d 44 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 45
asobhy 0:a355e511bc5d 46 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 47 // in seconds between interrupts, and start interrupt generation:
asobhy 0:a355e511bc5d 48 PeriodicInt.attach(&PeriodicInterruptISR, 0.05);
asobhy 0:a355e511bc5d 49 }
asobhy 0:a355e511bc5d 50
asobhy 0:a355e511bc5d 51
asobhy 0:a355e511bc5d 52 /*******************************************************************************
asobhy 0:a355e511bc5d 53 * ******** Periodic Timer Interrupt Thread ********
asobhy 0:a355e511bc5d 54 *******************************************************************************/
asobhy 0:a355e511bc5d 55 void PiControlThread(void const *argument)
asobhy 0:a355e511bc5d 56 {
asobhy 0:a355e511bc5d 57 while (true)
asobhy 0:a355e511bc5d 58 {
asobhy 0:a355e511bc5d 59
asobhy 0:a355e511bc5d 60 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 61 led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled.
asobhy 0:a355e511bc5d 62
asobhy 0:a355e511bc5d 63 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 64 DE0_read(&ID, &dPosition, &dTime);
asobhy 0:a355e511bc5d 65
asobhy 0:a355e511bc5d 66 setpoint_mutex.lock();
asobhy 0:a355e511bc5d 67 e = setpoint-dPosition; // e is the velocity error
asobhy 0:a355e511bc5d 68 setpoint_mutex.unlock();
asobhy 0:a355e511bc5d 69
asobhy 0:a355e511bc5d 70 xState = xState + e; // x is the Euler approximation to the integral of e.
asobhy 0:a355e511bc5d 71 u = Kp*e + Ki*xState; // u is the control signal
asobhy 0:a355e511bc5d 72
asobhy 0:a355e511bc5d 73
asobhy 0:a355e511bc5d 74 if (u >= 0)
asobhy 0:a355e511bc5d 75 {
asobhy 0:a355e511bc5d 76 motorDriver_forward(u);
asobhy 0:a355e511bc5d 77 }
asobhy 0:a355e511bc5d 78 else if (u < 0)
asobhy 0:a355e511bc5d 79 {
asobhy 0:a355e511bc5d 80 motorDriver_reverse(u);
asobhy 0:a355e511bc5d 81 }
asobhy 0:a355e511bc5d 82 else
asobhy 0:a355e511bc5d 83 {
asobhy 0:a355e511bc5d 84 pc.printf("\r\nerror!!!");
asobhy 0:a355e511bc5d 85 }
asobhy 0:a355e511bc5d 86 }
asobhy 0:a355e511bc5d 87 }
asobhy 0:a355e511bc5d 88
asobhy 0:a355e511bc5d 89 /*******************************************************************************
asobhy 0:a355e511bc5d 90 * the interrupt below occures every 250ms as setup in the main function during
asobhy 0:a355e511bc5d 91 * initialization
asobhy 0:a355e511bc5d 92 * ******** Period Timer Interrupt Handler ********
asobhy 0:a355e511bc5d 93 *******************************************************************************/
asobhy 0:a355e511bc5d 94 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 95 {
asobhy 0:a355e511bc5d 96 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 97 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 98 }