The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
Diff: main.cpp
- Revision:
- 2:2bc519e14bae
- Parent:
- 1:cc5636894b95
- Child:
- 3:dfb6733ae397
diff -r cc5636894b95 -r 2bc519e14bae main.cpp --- a/main.cpp Sun Mar 13 15:22:39 2016 +0000 +++ b/main.cpp Sun Mar 13 17:55:42 2016 +0000 @@ -1,318 +1,106 @@ -// Description: combines the functionality of both the MotorControl program that controls a motor and the Pixy subsystem -// to create a program that will control the robot - -#include "rtos.h" -#include "mbed.h" -#include "Robot_Control.h" -#include "Pixy.h" - -#define MAX_BLOCKS 1 -#define TARGET_DECIMAL 0x10 //the code of our target in decimal - -//controls whether or not motors are a part of the program -//for our test of the PIXY, they will not be -#define MOTORS_ON 0 +// Includes +// ---------------------------------------------------------------- -//steering inputs, may want to change to variables or something -#define STEER_SETPOINT 160 -#define STEER_KP 1 -#define STEER_KI 0 -#define STEER_MAX 2147483648 +#include "global.h" +#include "robot.h" +#include "response.h" +#include "tuning.h" -//speed inputs -#define HEIGHT_SETPOINT 160 -#define SPEED_KP 1 -#define SPEED_KI 0 -#define SPEED_MAX 2147483648 - -//shared variables between sensor thread and motor thread, protected by mutex -float left_setpoint, right_setpoint; -//the setpoint mutex -osMutexId setpoint_mutex; -osMutexDef(setpoint_mutex); - -//motorPI function variables -float L_xState, R_xState; //the states for the left and right pi control loop (must have them outside of function to make them cumulative) - -//PI function variables -float steer_xState, speed_xState; //the states for the steering and speed pi control loop - -//main()variables -short idCode; -char isMeasuring; -char inKey; -float pwmPeriod = 0.001; +// Definitions +// ---------------------------------------------------------------- +enum Mode { + AUTO_TRACK, + MANUAL_CONTROL, + PIXY_SUBSYSTEM, + MOTOR_RESPONSE, + SPEED_RESPONSE, + STEERING_RESPONSE, + TILT_RESPONSE, + MOTOR_TUNING, + SPEED_TUNING, + STEERING_TUNING, + TILT_TUNING +}; -//global variables -float Kp, Ki; //kp and ki for motors +void PrintMenu(Serial *pc); -// Various debugging variables -float vels[60], times[60], errs[60], xvals[60], uvals[60]; // Used to measure step response -int x_global, height_global; -float speed_global, steer_global; -// ******** Main Thread ******** -int main() { // This thread executes first upon reset or power-on. - - if(MOTORS_ON){ - // left - PWM0.period(pwmPeriod); - PWM0.pulsewidth(0); +// Wiring - TODO +// ---------------------------------------------------------------- +// +// +// +// +// +// - // right - PWM1.period(pwmPeriod); - PWM1.pulsewidth(0); - - //mutex for the left and right motor setpoints - setpoint_mutex = osMutexCreate(osMutex(setpoint_mutex)); - } - - Bumper.rise(&ExtCollisionISR); // Attach the address of the interrupt handler to the rising edge of Bumper - - // Start execution of the threads: MotorControlThread,ExtCollisionThread, and SensorControlThread: - SensorControl = osThreadCreate(osThread(SensorControlThread), NULL); - if(MOTORS_ON) MotorControl = osThreadCreate(osThread(MotorControlThread), NULL); - ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL); - - // Start the watch dog timer and enable the watch dog interrupt - osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0); - led3=0; // Clear watch dog led3 status - - if(MOTORS_ON){ - //SPI Section - pc.printf("\n\rStarting SPI..."); - // Restart with a new SPI protocol. - DE0.format(16,1); // 16 bit mode 1 - IoReset = 0; - IoReset = 1; - wait_us(10); - IoReset = 0; - SpiReset = 0; - SpiReset = 1; - wait_us(10); - SpiReset = 0; - idCode = DE0.write(0x8004); // Read count/time for both motors - pc.printf("\n\rDevice Id: %d", idCode); - - // motor Kp and Ki - Kp = 0.000120; - Ki = 0.0000000001; - } - - // Specify address of the ISRs and intervals in seconds between interrupts - // TODO: Optimize interrupt time for motor... Currently too fast - if(MOTORS_ON) MotorPeriodicInt.attach(&MotorControllerISR, .001); - SensorPeriodicInt.attach(&SensorControllerISR, .0167); //trigger sensor thread 60 times/sec - - //isMeasuring = 1; - //Setpoint = 6; - +// Main Program +// ---------------------------------------------------------------- +int main() { + char mode; while (1) { - - osTimerStart(OneShot, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. - - pc.printf("X Coordinate: %d, Steering: %d,", x_global, steer_global); - pc.printf("Height: %d, Speed: %d,", height_global, speed_global); - pc.printf(" L setpoint: %d, R setpoint: %d \n\r", left_setpoint, right_setpoint); - - /* - if (!isMeasuring && (Setpoint!=0)) { - pc.printf("\n\r-----------------------------------\n\r"); - printf("times (ms) \t vel (rads/s) \t error \t\t x value \t u value \n\r"); - for (i=0; i<60; i++) { - printf("%f, \t %f, \t %f, \t %f, \t %f \n\r", times[i], vels[i], errs[i], xvals[i], uvals[i]); - } - pc.printf("\n\r-----------------------------------\n\r"); - Setpoint = 0; - i = 0; + PrintMenu(&pc); + mode = pc.getc(); + switch (mode) { + case AUTO_TRACK: + AutoTrack(); + break; + case MANUAL_CONTROL: + ManualControl(); + break; + case PIXY_SUBSYSTEM: + PixySubsystem(); + break; + case MOTOR_RESPONSE: + MotorResponse(); + break; + case SPEED_RESPONSE: + SpeedResponse(); + break; + case STEERING_RESPONSE: + SteeringResponse(); + break; + case TILT_RESPONSE: + TiltResponse(); + break; + case MOTOR_TUNING: + MotorTuning(); + break; + case SPEED_TUNING: + SpeedTuning(); + break; + case STEERING_TUNING: + SteeringTuning(); + break; + case TILT_TUNING: + TiltTuning(); + break; + default: + pc.printf("\r\n\r\n Error: Invalid Selection \r\n\r\n"); + wait_ms(2000); + break; } - */ - Thread::wait(500); // Go to sleep for 500 ms + PrintMenu(&pc); } } -// ******** Control Thread ******** -void SensorControlThread(void const *argument) { - float speed, steer; - int x, count, height; - - Pixy pixy(Pixy::SPI, p11, p12, p13); - - while (1) { - osSignalWait(SignalSensor, osWaitForever); //note: SignalSensor is just zero, waits for any signal dirrected at thread. - - count = pixy.getBlocks(130); //get as many blocks as are available - - //find the target block in all of the blocks that were sent - for(x=0;x<count;x++){ - if (pixy.blocks[x].signature == TARGET_DECIMAL) break; - if (x==count) pc.printf("no target in frame"); - } - - // if blocks were returned - if (count){ - - //height = CalcHeight() //find the feedback from the pixy for the height - height = pixy.blocks[(x-1)].height; //use this for now - - steer = PI(0, STEER_SETPOINT, STEER_KP, STEER_KI, pixy.blocks[(x-1)].x, STEER_MAX); //the feedback for the steering is the x location of the target block - speed = PI(1, HEIGHT_SETPOINT, SPEED_KP, SPEED_KI, height, SPEED_MAX); - - if(!MOTORS_ON){ - x_global = pixy.blocks[x-1].x; - height_global = height; - steer_global = steer; - speed_global = speed; - } - - // give setpoints to MotorControlThread - osMutexWait(setpoint_mutex, osWaitForever); //enter mutex - left_setpoint = speed - steer; - right_setpoint = speed + steer; - osMutexRelease(setpoint_mutex); //exit mutex - } - - } -} +// Other Functions +// ---------------------------------------------------------------- +void PrintMenu(Serial *pc){ + pc->printf("---------------------------------------------------------------- \r\n"); + pc->printf("0. Automated Tracking \r\n"); + pc->printf("1. Manual Control \r\n"); + pc->printf("2. Pixy Subsystem \r\n"); + pc->printf("3. Motor Response \r\n"); + pc->printf("4. Speed Response \r\n"); + pc->printf("5. Steering Response \r\n"); + pc->printf("6. Tilt Response \r\n"); + pc->printf("7. Tune Motor \r\n"); + pc->printf("8. Tune Speed \r\n"); + pc->printf("9. Tune Steering \r\n"); + pc->printf("a. Tune Tilt \r\n"); + pc->printf("---------------------------------------------------------------- \r\n\r\n"); -// ******** Control Thread ******** -void MotorControlThread(void const *argument) { - short dP_L, dt_L, dP_R, dt_R; - float uL,uR; - float left_set,right_set; - - while (1) { - osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received. SignalPi is just zero lol, waits for any signal to thread - led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled. - - dP_L = DE0.write(0); // Read QEI-0 position register - dt_L = DE0.write(0); // Read QEI-0 time register - dP_R = DE0.write(0); // Read QEI-0 position register - dt_R = DE0.write(0); // Read QEI-0 time register - - osMutexWait(setpoint_mutex, osWaitForever);//enter mutex - left_set= left_setpoint; //copy setpoint - right_set= right_setpoint; - osMutexRelease(setpoint_mutex);//exit mutex - - uL = motorPI(0, left_set, Kp, Ki, dP_L, dt_L); - uR = motorPI(1, right_set, Kp, Ki, dP_R, dt_R); - - PWM0.pulsewidth(fabs(uL)); - PWM1.pulsewidth(fabs(uR)); - } -} - -// ******** Collision Thread ******** -void ExtCollisionThread(void const *argument) { - while (1) { - osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received - led4 = 1; - } -} - -// ******** Watchdog Interrupt Handler ******** -// TODO: Shutdown system -void Watchdog(void const *n) { - led3=1; // led3 is activated when the watchdog timer times out -} - -// ******** Period Timer Interrupt Handler ******** -void MotorControllerISR(void) { - osSignalSet(MotorControl,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt. + pc->printf("=> "); } - -// ******** Period Timer Interrupt Handler ******** -void SensorControllerISR(void) { - osSignalSet(SensorControl,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt. (basically sends signal to id of thread) -} - -// ******** Collision Interrupt Handler ******** -void ExtCollisionISR(void) { - osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt. -} - -// motorPI - motor pi control extacted from previous work -// Input: 0 for Left motor, 1 for right motor -// Output: pwm pulsewidth for corresponding motor - -float motorPI(int RL_Motor, float Setpoint, float Kp, float Ki, short dP1, short dt1){ - int dP32, i; - float angVel, e, u, xState; - - if (dP1 & 0x00008000) { - dP32 = dP1 | 0xFFFF0000; - } else { - dP32 = dP1; - } - - angVel = QE2RadsPerSec(dP32, dt1); - - e = Setpoint - angVel; - - RL_Motor ? xState = R_xState : xState = L_xState; //state is cumulative, select left or right state global variable - - // Avoid integrator wind up - if((u>=pwmPeriod)||(u<=-pwmPeriod)){ - //xState= xState; - turns off integrator when u maxed - }else{ - xState = xState + e; - } - - RL_Motor ? R_xState = xState : L_xState = xState; //update state variable - - u = Ki*xState + Kp*e; - - // Set direction and pwm - if (u >= 0) { - RL_Motor ? DIR1 = 1 : DIR0 = 1; - } else { - RL_Motor ? DIR1 = 0 : DIR0 = 0; - } - - if (isMeasuring) { - vels[i] = angVel; - times[i] = i; - errs[i] = e; - xvals[i] = xState; - uvals[i] = u; - if (i>=59) { - isMeasuring = 0; - } - i++; - } - - if (fabs(u) > pwmPeriod) u = pwmPeriod; - - return u; -} - -// PI() - A generic PI controller function used by the sensor control thread -// st_or_sp - 0 for steering, 1 for speed -float PI(int st_or_sp, float setpoint, float Kp, float Ki, int feedback, float max_val){ - float e, xState, u; - - e = setpoint - (float)feedback; - - st_or_sp ? xState = speed_xState : xState = steer_xState; - - // Avoid integrator wind up - if((u>=max_val)||(u<=-max_val)){ - //xState= xState; - turns off integrator when u maxed - }else{ - xState = xState + e; - } - - st_or_sp ? speed_xState = xState : steer_xState = xState; //update state variable - - u = Ki*xState + Kp*e; - - if (fabs(u) > max_val) u = max_val; - - return u; -} - -float QE2RadsPerSec(int counts, int time) { - return (counts*122.62)/time; -} \ No newline at end of file