ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@8:a0890fa79084, 2018-02-19 (annotated)

Committer:
asobhy
Date:
Mon Feb 19 17:42:33 2018 +0000
Revision:
8:a0890fa79084
Parent:
7:73fd05fe556a
Child:
9:fe56b888985c
added more comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 8:a0890fa79084 1 /******************************************************************************/
asobhy 8:a0890fa79084 2 // ECE4333
asobhy 8:a0890fa79084 3 // LAB Partner 1: Ahmed Sobhy - ID: 3594449
asobhy 8:a0890fa79084 4 // LAB Partner 2: Brandon Kingman - ID: 3470444
asobhy 8:a0890fa79084 5 // Project: Autonomous Robot Design
asobhy 8:a0890fa79084 6 // Instructor: Prof. Chris Diduch
asobhy 8:a0890fa79084 7 /******************************************************************************/
asobhy 8:a0890fa79084 8 // filename: PiControlThread.cpp
asobhy 8:a0890fa79084 9 // file content description:
asobhy 8:a0890fa79084 10 // * Function to Create the PiControl Thread
asobhy 8:a0890fa79084 11 // * PiControl Thread
asobhy 8:a0890fa79084 12 // * PiControl ISR
asobhy 8:a0890fa79084 13 // * Variables related to the functionality of the thread
asobhy 8:a0890fa79084 14 /******************************************************************************/
asobhy 8:a0890fa79084 15
asobhy 0:a355e511bc5d 16 #include "mbed.h"
asobhy 0:a355e511bc5d 17 #include "ui.h"
asobhy 0:a355e511bc5d 18 #include "Drivers/motor_driver.h"
asobhy 0:a355e511bc5d 19 #include "Drivers/DE0_driver.h"
asobhy 1:3e9684e81312 20 #include "PiControlThread.h"
asobhy 7:73fd05fe556a 21 #include "Drivers/PiController.h"
asobhy 0:a355e511bc5d 22
asobhy 0:a355e511bc5d 23 extern int setpoint;
asobhy 0:a355e511bc5d 24
asobhy 1:3e9684e81312 25 uint16_t ID, dTime;
asobhy 1:3e9684e81312 26 int dPosition;
asobhy 1:3e9684e81312 27 int vel;
asobhy 4:417e475239c7 28 int32_t U;
asobhy 0:a355e511bc5d 29
asobhy 8:a0890fa79084 30 int time_passed = 0;
asobhy 8:a0890fa79084 31
asobhy 0:a355e511bc5d 32 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 33 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 34
asobhy 0:a355e511bc5d 35 osThreadId PiControlId;
asobhy 0:a355e511bc5d 36
asobhy 0:a355e511bc5d 37 /******************************************************************************/
asobhy 0:a355e511bc5d 38 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 39 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 40 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 41 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 42 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 43 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 44 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 45 /******************************************************************************/
asobhy 0:a355e511bc5d 46
asobhy 0:a355e511bc5d 47 // Declare PeriodicInterruptThread as a thread/process
asobhy 0:a355e511bc5d 48 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 49
asobhy 0:a355e511bc5d 50 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 51
asobhy 0:a355e511bc5d 52 DigitalOut led3(LED3);
asobhy 0:a355e511bc5d 53
asobhy 7:73fd05fe556a 54 /*******************************************************************************
asobhy 7:73fd05fe556a 55 * @brief function that creates a thread for the PI controller. It initializes
asobhy 7:73fd05fe556a 56 * the PI controller's gains and initializes the DC Motor. It also
asobhy 7:73fd05fe556a 57 * initializes the PIControllerThread runs at 50ms period
asobhy 7:73fd05fe556a 58 * @param none
asobhy 7:73fd05fe556a 59 * @return none
asobhy 7:73fd05fe556a 60 *******************************************************************************/
asobhy 0:a355e511bc5d 61 void PiControlThreadInit()
asobhy 0:a355e511bc5d 62 {
asobhy 4:417e475239c7 63 DE0_init(); // initialize FPGA
asobhy 4:417e475239c7 64 motorDriver_init(); // initialize motorDriver
asobhy 6:e7ce340fe91e 65 // Kp,Ki
asobhy 6:e7ce340fe91e 66 PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
asobhy 0:a355e511bc5d 67
asobhy 0:a355e511bc5d 68 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 69
asobhy 0:a355e511bc5d 70 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 71 // in seconds between interrupts, and start interrupt generation:
asobhy 1:3e9684e81312 72 PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate
asobhy 4:417e475239c7 73
asobhy 0:a355e511bc5d 74 }
asobhy 0:a355e511bc5d 75
asobhy 0:a355e511bc5d 76
asobhy 0:a355e511bc5d 77 /*******************************************************************************
asobhy 7:73fd05fe556a 78 * @brief This is the PI controller thread. It reads several values from the
asobhy 7:73fd05fe556a 79 * FPGA such as speed, time and other sensors data
asobhy 7:73fd05fe556a 80 * @param none
asobhy 7:73fd05fe556a 81 * @return none
asobhy 0:a355e511bc5d 82 *******************************************************************************/
asobhy 0:a355e511bc5d 83 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 84 {
asobhy 3:4def4ca68910 85
asobhy 0:a355e511bc5d 86 while (true)
asobhy 0:a355e511bc5d 87 {
asobhy 0:a355e511bc5d 88 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 89
asobhy 8:a0890fa79084 90 time_passed++;
asobhy 8:a0890fa79084 91
asobhy 0:a355e511bc5d 92 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 93 DE0_read(&ID, &dPosition, &dTime);
asobhy 1:3e9684e81312 94 SaturateValue(dPosition, 560);
asobhy 1:3e9684e81312 95
asobhy 1:3e9684e81312 96 // maximum velocity at dPostition = 560 is vel = 703
asobhy 1:3e9684e81312 97 vel = (float)((6135.92 * dPosition) / dTime) ;
asobhy 0:a355e511bc5d 98
asobhy 4:417e475239c7 99 U = PiController(setpoint,dPosition);
asobhy 1:3e9684e81312 100
asobhy 4:417e475239c7 101 if (U >= 0)
asobhy 0:a355e511bc5d 102 {
asobhy 4:417e475239c7 103 motorDriver_forward(U);
asobhy 0:a355e511bc5d 104 }
asobhy 4:417e475239c7 105 else if (U < 0)
asobhy 0:a355e511bc5d 106 {
asobhy 4:417e475239c7 107 motorDriver_reverse(U);
asobhy 7:73fd05fe556a 108 }
asobhy 3:4def4ca68910 109
asobhy 0:a355e511bc5d 110 }
asobhy 1:3e9684e81312 111
asobhy 0:a355e511bc5d 112 }
asobhy 0:a355e511bc5d 113
asobhy 0:a355e511bc5d 114 /*******************************************************************************
asobhy 7:73fd05fe556a 115 * @brief The ISR below signals the PIControllerThread. it is setup to run
asobhy 7:73fd05fe556a 116 * every 50ms
asobhy 7:73fd05fe556a 117 * @param none
asobhy 7:73fd05fe556a 118 * @return none
asobhy 0:a355e511bc5d 119 *******************************************************************************/
asobhy 0:a355e511bc5d 120 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 121 {
asobhy 0:a355e511bc5d 122 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 123 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 124 }
asobhy 1:3e9684e81312 125