Diff: main.cpp

Revision:

0:80a37292f6b2

Child:

1:cc5636894b95

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 09 02:45:40 2016 +0000
@@ -0,0 +1,320 @@
+// Description: combines the functionality of both the PIcontrol 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 12 //the code of our target, very important, should this be octal...?
+
+//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
+
+//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
+
+//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;
+
+//global variables
+float Kp, Ki; //kp and ki for motors
+
+// 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);
+        Thread::wait(500);
+
+        // right
+        PWM1.period(pwmPeriod);
+        PWM1.pulsewidth(0);
+        Thread::wait(500);
+   
+        //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: PiControlThread,ExtCollisionThread, and SensorControlThread:
+    SensorControl = osThreadCreate(osThread(SensorControlThread), NULL);
+    if(MOTORS_ON) PiControl = osThreadCreate(osThread(PiControlThread), 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); //changed from 8002 for one motor to 8004 for two
+        pc.printf("\n\rDevice Id: %d", idCode);
+    
+        // motor Kp and Ki
+        Kp = 0.000120;
+        Ki = 0.0000000001;
+    }
+    
+    // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval 
+    // in seconds between interrupts, and start interrupt generation:
+    if(MOTORS_ON) PeriodicInt.attach(&PiControllerISR, .001);
+    PeriodicInt2.attach(&SensorControllerISR, .0167); //trigger sensor thread 60 times/sec
+    
+    //isMeasuring = 1; 
+    //Setpoint = 6; 
+    
+    while (1) {
+        
+        osTimerStart(OneShot, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
+
+        if(!MOTORS_ON){
+            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;
+        }
+        */
+        Thread::wait(500); // Go to sleep for 500 ms
+    }
+}
+
+// ******** 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) 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 PiControlThread
+            osMutexWait(setpoint_mutex, osWaitForever); //enter mutex
+            left_setpoint = speed - steer;
+            right_setpoint = speed + steer;
+            osMutexRelease(setpoint_mutex); //exit mutex
+        }
+
+    } 
+}
+    
+// ******** Control Thread ********
+void PiControlThread(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 PiControlThread 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 ********
+void Watchdog(void const *n) {
+    led3=1; // led3 is activated when the watchdog timer times out
+}
+
+// ******** Period Timer Interrupt Handler ********
+void PiControllerISR(void) {
+    osSignalSet(PiControl,0x1); // Activate the signal, PiControl, with each periodic timer interrupt.
+}
+
+// ******** 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