![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ManualControlFinal by
PiControlThread.cpp
- Committer:
- asobhy
- Date:
- 2018-04-10
- Branch:
- ManualControl
- Revision:
- 29:83c103d12078
- Parent:
- 22:c09acff62e6a
- Child:
- 23:1839085ffdcf
File content as of revision 29:83c103d12078:
/******************************************************************************/ // ECE4333 // LAB Partner 1: Ahmed Sobhy - ID: 3594449 // LAB Partner 2: Brandon Kingman - ID: 3470444 // Project: Autonomous Robot Design // Instructor: Prof. Chris Diduch /******************************************************************************/ // filename: PiControlThread.cpp // file content description: // * Function to Create the PiControl Thread // * PiControl Thread // * PiControl ISR // * Variables related to the functionality of the thread /******************************************************************************/ #include "mbed.h" #include "ui.h" #include "Drivers/motor_driver_r.h" #include "Drivers/motor_driver_l.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" #include "Drivers/PiController.h" #include "ui.h" #include "CameraThread.h" // setpoints // speed int setpointR = 0; int setpointL = 0; // int velR, velL; // control signal int32_t U_right, U_left; sensors_t sensors; void PiControlThread(void const *); void PeriodicInterruptISR(void); // steering gain float Ks = 0.15; // distance gain float Kd = 10; // overall robot required speed int Setpoint; Mutex mutexSetpoint; osThreadId PiControlId; /******************************************************************************/ // osPriorityIdle = -3, ///< priority: idle (lowest) // osPriorityLow = -2, ///< priority: low // osPriorityBelowNormal = -1, ///< priority: below normal // osPriorityNormal = 0, ///< priority: normal (default) // osPriorityAboveNormal = +1, ///< priority: above normal // osPriorityHigh = +2, ///< priority: high // osPriorityRealtime = +3, ///< priority: realtime (highest) /******************************************************************************/ // Declare PeriodicInterruptThread as a thread/process osThreadDef(PiControlThread, osPriorityRealtime, 1024); Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt /******************************************************************************* * @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 motorDriver_R_init(); // initialize motorDriver motorDriver_L_init(); // initialize motorDriver // Kp,Ki PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables PiControlId = osThreadCreate(osThread(PiControlThread), NULL); // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: PeriodicInt.attach(&PeriodicInterruptISR, 0.010); // 10ms sampling period -> 100Hz freq } /******************************************************************************* * @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) { while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. // Get incremental position and time from QEI DE0_read(&sensors); // might not be needed sensors.dp_right = SaturateValue(sensors.dp_right, 112); sensors.dp_left = SaturateValue(sensors.dp_left, 112); // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; /********************Manual Setpoint and Steering**********************/ mutexSetpoint.lock(); setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); mutexSetpoint.unlock(); // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 112); setpointL = SaturateValue(setpointL, 112); /*********************Differential End*********************************/ U_right = PiControllerR(setpointR,sensors.dp_right); U_left = PiControllerL(setpointL,sensors.dp_left); // Set speed and direction for right motor if (U_right >= 0) { motorDriver_R_forward(U_right); } else if (U_right < 0) { motorDriver_R_reverse(U_right); } // Set speed and direction for left motor if (U_left >= 0) { motorDriver_L_forward(U_left); } else if (U_left < 0) { motorDriver_L_reverse(U_left); } } } /******************************************************************************* * @brief The ISR below signals the PIControllerThread. it is setup to run * every 10ms * @param none * @return none *******************************************************************************/ void PeriodicInterruptISR(void) { // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. osSignalSet(PiControlId,0x1); }