Diff: main.cpp

Revision:

2:2bc519e14bae

Parent:

1:cc5636894b95

Child:

3:dfb6733ae397

--- 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