ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@9:fe56b888985c, 2018-02-23 (annotated)

Committer:
asobhy
Date:
Fri Feb 23 20:58:34 2018 +0000
Revision:
9:fe56b888985c
Parent:
8:a0890fa79084
Child:
10:8919b1b76243
right after the two motors are running

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 8:a0890fa79084 1 /******************************************************************************/
asobhy 8:a0890fa79084 2 // ECE4333
asobhy 9:fe56b888985c 3 // LAB Partner 1: Ahmed Sobhy - ID: 3594449
asobhy 9:fe56b888985c 4 // LAB Partner 2: Brandon Kingman - ID: 3470444
asobhy 9:fe56b888985c 5 // Project: Autonomous Robot Design
asobhy 9:fe56b888985c 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 9:fe56b888985c 18 #include "Drivers/motor_driver_r.h"
asobhy 9:fe56b888985c 19 #include "Drivers/motor_driver_l.h"
asobhy 0:a355e511bc5d 20 #include "Drivers/DE0_driver.h"
asobhy 1:3e9684e81312 21 #include "PiControlThread.h"
asobhy 7:73fd05fe556a 22 #include "Drivers/PiController.h"
asobhy 0:a355e511bc5d 23
asobhy 9:fe56b888985c 24 extern int setpointR, setpointL;
asobhy 0:a355e511bc5d 25
asobhy 9:fe56b888985c 26 int velR, velL;
asobhy 9:fe56b888985c 27 int32_t U_right, U_left;
asobhy 9:fe56b888985c 28
asobhy 9:fe56b888985c 29 sensors_t sensors;
asobhy 0:a355e511bc5d 30
asobhy 8:a0890fa79084 31 int time_passed = 0;
asobhy 8:a0890fa79084 32
asobhy 0:a355e511bc5d 33 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 34 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 35
asobhy 0:a355e511bc5d 36 osThreadId PiControlId;
asobhy 0:a355e511bc5d 37
asobhy 0:a355e511bc5d 38 /******************************************************************************/
asobhy 0:a355e511bc5d 39 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 40 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 41 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 42 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 43 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 44 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 45 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 46 /******************************************************************************/
asobhy 0:a355e511bc5d 47
asobhy 0:a355e511bc5d 48 // Declare PeriodicInterruptThread as a thread/process
asobhy 0:a355e511bc5d 49 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 50
asobhy 0:a355e511bc5d 51 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 52
asobhy 0:a355e511bc5d 53 DigitalOut led3(LED3);
asobhy 0:a355e511bc5d 54
asobhy 7:73fd05fe556a 55 /*******************************************************************************
asobhy 7:73fd05fe556a 56 * @brief function that creates a thread for the PI controller. It initializes
asobhy 7:73fd05fe556a 57 * the PI controller's gains and initializes the DC Motor. It also
asobhy 7:73fd05fe556a 58 * initializes the PIControllerThread runs at 50ms period
asobhy 7:73fd05fe556a 59 * @param none
asobhy 7:73fd05fe556a 60 * @return none
asobhy 7:73fd05fe556a 61 *******************************************************************************/
asobhy 0:a355e511bc5d 62 void PiControlThreadInit()
asobhy 0:a355e511bc5d 63 {
asobhy 4:417e475239c7 64 DE0_init(); // initialize FPGA
asobhy 9:fe56b888985c 65 motorDriver_R_init(); // initialize motorDriver
asobhy 9:fe56b888985c 66 motorDriver_L_init(); // initialize motorDriver
asobhy 6:e7ce340fe91e 67 // Kp,Ki
asobhy 6:e7ce340fe91e 68 PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
asobhy 0:a355e511bc5d 69
asobhy 0:a355e511bc5d 70 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 71
asobhy 0:a355e511bc5d 72 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 73 // in seconds between interrupts, and start interrupt generation:
asobhy 1:3e9684e81312 74 PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate
asobhy 4:417e475239c7 75
asobhy 0:a355e511bc5d 76 }
asobhy 0:a355e511bc5d 77
asobhy 0:a355e511bc5d 78
asobhy 0:a355e511bc5d 79 /*******************************************************************************
asobhy 7:73fd05fe556a 80 * @brief This is the PI controller thread. It reads several values from the
asobhy 7:73fd05fe556a 81 * FPGA such as speed, time and other sensors data
asobhy 7:73fd05fe556a 82 * @param none
asobhy 7:73fd05fe556a 83 * @return none
asobhy 0:a355e511bc5d 84 *******************************************************************************/
asobhy 0:a355e511bc5d 85 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 86 {
asobhy 3:4def4ca68910 87
asobhy 0:a355e511bc5d 88 while (true)
asobhy 0:a355e511bc5d 89 {
asobhy 0:a355e511bc5d 90 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 91
asobhy 9:fe56b888985c 92 //time_passed++;
asobhy 8:a0890fa79084 93
asobhy 0:a355e511bc5d 94 // get incremental position and time from QEI
asobhy 9:fe56b888985c 95 DE0_read(&sensors);
asobhy 9:fe56b888985c 96
asobhy 9:fe56b888985c 97 SaturateValue(sensors.dp_right, 560);
asobhy 9:fe56b888985c 98 SaturateValue(sensors.dp_left, 560);
asobhy 9:fe56b888985c 99
asobhy 9:fe56b888985c 100 // maximum velocity at dPostition = 560 is vel = 703
asobhy 9:fe56b888985c 101 velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
asobhy 1:3e9684e81312 102
asobhy 1:3e9684e81312 103 // maximum velocity at dPostition = 560 is vel = 703
asobhy 9:fe56b888985c 104 velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
asobhy 9:fe56b888985c 105
asobhy 0:a355e511bc5d 106
asobhy 9:fe56b888985c 107 U_right = PiControllerR(setpointR,sensors.dp_right);
asobhy 9:fe56b888985c 108 U_left = PiControllerL(setpointL,sensors.dp_left);
asobhy 1:3e9684e81312 109
asobhy 9:fe56b888985c 110 // set speed and direction for right motor
asobhy 9:fe56b888985c 111 if (U_right >= 0)
asobhy 0:a355e511bc5d 112 {
asobhy 9:fe56b888985c 113 motorDriver_R_forward(U_right);
asobhy 9:fe56b888985c 114 }
asobhy 9:fe56b888985c 115 else if (U_right < 0)
asobhy 9:fe56b888985c 116 {
asobhy 9:fe56b888985c 117 motorDriver_R_reverse(U_right);
asobhy 0:a355e511bc5d 118 }
asobhy 9:fe56b888985c 119
asobhy 9:fe56b888985c 120 // set speed and direction of left motor
asobhy 9:fe56b888985c 121 if (U_left >= 0)
asobhy 0:a355e511bc5d 122 {
asobhy 9:fe56b888985c 123 motorDriver_L_forward(U_left);
asobhy 9:fe56b888985c 124 }
asobhy 9:fe56b888985c 125 else if (U_left < 0)
asobhy 9:fe56b888985c 126 {
asobhy 9:fe56b888985c 127 motorDriver_L_reverse(U_left);
asobhy 7:73fd05fe556a 128 }
asobhy 3:4def4ca68910 129
asobhy 0:a355e511bc5d 130 }
asobhy 1:3e9684e81312 131
asobhy 0:a355e511bc5d 132 }
asobhy 0:a355e511bc5d 133
asobhy 0:a355e511bc5d 134 /*******************************************************************************
asobhy 7:73fd05fe556a 135 * @brief The ISR below signals the PIControllerThread. it is setup to run
asobhy 7:73fd05fe556a 136 * every 50ms
asobhy 7:73fd05fe556a 137 * @param none
asobhy 7:73fd05fe556a 138 * @return none
asobhy 0:a355e511bc5d 139 *******************************************************************************/
asobhy 0:a355e511bc5d 140 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 141 {
asobhy 0:a355e511bc5d 142 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 143 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 144 }
asobhy 1:3e9684e81312 145