![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ManualControlFinal by
PiControlThread.cpp
- Committer:
- asobhy
- Date:
- 2018-03-29
- Revision:
- 23:1839085ffdcf
- Parent:
- 22:c09acff62e6a
File content as of revision 23:1839085ffdcf:
/******************************************************************************/ // 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; dp_t dpArray; int iEnd; bool memoryFull = false; bool patrolStart = true; bool reverse = true; bool do180 = true; int cnt=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) { dpArray.i = 0; dpArray.size = ARRAY_SIZE; 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) ; // if manual control is ON use Differential drive if(MC) { /******************Manual Setpoint and Steering********************/ mutexSetpoint.lock(); setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); mutexSetpoint.unlock(); /******************Differential End********************************/ // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 112); setpointL = SaturateValue(setpointL, 112); /**********************Record Start********************************/ // Record dp if there is room in memory if( (dpArray.i < dpArray.size) ) { // only record data when record button is pressed if(startRecording) { dpArray.dpR[dpArray.i] = setpointR; dpArray.dpL[dpArray.i] = setpointL; dpArray.i++; patrolStart = true; } } else { // memory is full memoryFull = true; } /************************Record End********************************/ /*********************Playback Start*******************************/ } else { // if manual control is not ON then Playback data // Check if its the first time to enter the patrol mode if(patrolStart) { // Store end index value iEnd = dpArray.i; // Reset playback index dpArray.i = 0; patrolStart = false; } // if do the 180 degree is true then do a 180 rotation if( do180 == true ) { // distance = 10ms * cnt * speed if ( cnt < 300 ) { setpointR = -10; setpointL = 10; cnt++; } // exit the 180 code when done else { do180 = false; cnt = 0; } } else if ( dpArray.i < iEnd ) { // Data Playback normal direction if(!reverse) { setpointR = dpArray.dpR[dpArray.i]; setpointL = dpArray.dpL[dpArray.i]; } // Data Playback in reverse direction else { // Wheel dps are swaped to mantain correct direction playback setpointR = dpArray.dpL[dpArray.i]; setpointL = dpArray.dpR[dpArray.i]; } dpArray.i++; } else { // We reached the end of our playback we need to toggle reverse reverse = !reverse; // start from the beginning by reseting the index dpArray.i = 0; // Do a 180 degree turn do180 = true; } /***********************Playback 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); }