![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ManualControlFinal by
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?
User | Revision | Line number | New 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 |