![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ObjectFollower by
PiControlThread.cpp
- Committer:
- asobhy
- Date:
- 2018-04-03
- Revision:
- 27:510b4af89a6b
- Parent:
- 25:f2a1bd6591fb
File content as of revision 27:510b4af89a6b:
/******************************************************************************/ // 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 // int velR, velL; // control signal int32_t U_right, U_left; sensors_t sensors; int time_passed = 0; void PiControlThread(void const *); void PeriodicInterruptISR(void); // steering gain float Ks = 0.12; // distance gain float Kd = 3; // overall robot required speed int Setpoint; 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 DigitalOut led3(LED3); /******************************************************************************* * @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.01); // 10ms sampling rate } /******************************************************************************* * @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 velocity at dPostition = 560 is vel = 703 //velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; // Maximum velocity at dPostition = 560 is vel = 703 //velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; /*********************Differential Start*******************************/ // Inputs are Speed Setpoint and Steering Setpoint // The Inputs for the Steering are specified in the CameraThread // and set at the center. // The distance between the object and the image should be set to 1 meter // If distance decrease then speed should stop. // If distance increase then speed should increase. // If object is moving away from the the robot increase robot speed cameraData_mutex.lock(); if(DistanceError > 0) { // Proportional controller Setpoint = (Kd*DistanceError); Setpoint = SaturateValue(Setpoint,112); } // If object is at the set distance limit or less then do not move. else if(DistanceError <= 0) { Setpoint = 0; } // Decoupled drive setpointL = Setpoint + (Ks*SteeringError); setpointR = Setpoint - (Ks*SteeringError); cameraData_mutex.unlock(); // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 112); setpointL = SaturateValue(setpointL, 112); /*********************Differential End*********************************/ /* the signs for dP are switched here in code instead of switching the * channels wires connected to the FPGA due to reversing the orientation * of the Robot */ 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 of 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 50ms * @param none * @return none *******************************************************************************/ void PeriodicInterruptISR(void) { // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. osSignalSet(PiControlId,0x1); }