File content as of revision 0:80a37292f6b2:

// 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;
}