File content as of revision 3:dfb6733ae397:

#include "robot.h"
#include "global.h"

// Global Definitions
// ----------------------------------------------------------------

#define MAX_BLOCKS 1
#define TARGET_DECIMAL 10 
#define PWM_PERIOD 0.001

#define MOTOR_KP 0.000120
#define MOTOR_KI 0.0000000001

#define X_SETPOINT 160
#define STEERING_KP 0.01
#define STEERING_KI 0.005

#define HEIGHT_SETPOINT 100
#define SPEED_KP 0.01
#define SPEED_KI 0.01
#define SPEED_MAX 10

// Interrupt and thread control
osThreadId motorId, navigationId, wdtId;
osThreadDef(MotorThread, osPriorityRealtime, DEFAULT_STACK_SIZE); 
osThreadDef(NavigationThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE); 
osTimerDef(Wdtimer, WatchdogISR);
int32_t motorSignal, navigationSignal;
Ticker motorPeriodicInt;  
Ticker sensorPeriodicInt;   

// Mutex to protect left and right motor setpoints
osMutexId motorMutex;
osMutexDef(motorMutex);

// Set points and display variables
float leftMotor, rightMotor;
int xTracked, heightTracked;
float speed, steering;

// PI internal variables (TODO: move to object)
float steeringIntegral, speedIntegral; 
float leftMotorIntegral, rightMotorIntegral; 


// Functions
// ----------------------------------------------------------------
void AutoTrack() { 
    
    leftPwm.period(PWM_PERIOD);
    leftPwm.pulsewidth(0);
    rightPwm.period(PWM_PERIOD);
    rightPwm.pulsewidth(0);

    pixy.setSerialOutput(&pc);
    motorMutex = osMutexCreate(osMutex(motorMutex));

    bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
    
    // Start execution of the threads MotorThread, and NavigationThread:
    navigationId = osThreadCreate(osThread(NavigationThread), NULL);
    motorId = osThreadCreate(osThread(MotorThread), NULL);
 
    // Start the watch dog timer and enable the watch dog interrupt
    osTimerId oneShotId = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
    led3 = 0; // Clear watch dog led3 status
    
    //SPI Initialization
    pc.printf("\n\rStarting SPI...");
    deSpi.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; 
    pc.printf("\n\rDevice Id: %d", deSpi.write(0x8004)); // Read count & time for both motors
    
    // Specify periodic ISRs and their intervals in seconds
    // TODO: Optimize interrupt time for motor... Currently too fast
    motorPeriodicInt.attach(&MotorISR, .001);
    sensorPeriodicInt.attach(&NavigationISR, 0.1); //trigger sensor thread 60 times/sec
    
    while (1) {
        
        osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.

        pc.printf("X Coordinate: %d, Height: %d \r\n", xTracked, heightTracked);
        pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
        pc.printf("Speed Int: %f, Steering Int: %f \r\n", speedIntegral, steeringIntegral);
        pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n", leftMotor, rightMotor);
        
        Thread::wait(500); // Go to sleep for 500 ms
    }
}

void ManualControl(void) {

}

void PixySubsystem(void) { 

}


// Threads
// ----------------------------------------------------------------
void NavigationThread(void const *argument) {
    int count;
    
    while (1) {
        osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR

        count = pixy.getBlocks(1);
        
        // If target returned
        if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
            heightTracked = pixy.blocks[0].height; //use this for now
            xTracked = pixy.blocks[0].x;

            PI(HEIGHT_SETPOINT-heightTracked, &speed, &speedIntegral, SPEED_KP, SPEED_KI, SPEED_MAX);
            PI(X_SETPOINT-xTracked, &steering, &steeringIntegral, STEERING_KP, STEERING_KI, 2*SPEED_MAX);

            // Give setpoints to MotorThread
            // Do not update speeds if already turning as fast as possible
            osMutexWait(motorMutex, osWaitForever); 
            leftMotor = speed - steering;
            rightMotor = speed + steering;
            osMutexRelease(motorMutex); 

        }
    } 
}

void MotorThread(void const *argument) {
    float leftSet, rightSet, angVel;
    short dP, dt;
    float uL, uR;

    while (1) {
        osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
        led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
        
        // Get setpoints from navigation
        osMutexWait(motorMutex, osWaitForever); 
        leftSet = leftMotor;
        rightSet = rightMotor;
        osMutexRelease(motorMutex); 
        
        // Run PI control on left motor
        dP = deSpi.write(0);
        dt = deSpi.write(0); 
        angVel = QE2RadsPerSec(dP, dt);
        PI(leftSet-angVel, &uL, &leftMotorIntegral, MOTOR_KP, MOTOR_KI, PWM_PERIOD);
        
        // Run PI control on right motor
        dP = deSpi.write(0);
        dt = deSpi.write(0); 
        angVel = QE2RadsPerSec(dP, dt);
        PI(rightSet-angVel, &uR, &rightMotorIntegral, MOTOR_KP, MOTOR_KI, PWM_PERIOD);
        
        // Output new PWM and direction (motors have opposite orientations)
        if (uL >= 0) leftDir = 1;   
        else leftDir = 0;
        
        if (uR >= 0) rightDir = 0;   
        else rightDir = 1;
        
        leftPwm.pulsewidth(fabs(uL));
        rightPwm.pulsewidth(fabs(uR));
    } 
}


// Interrupt Service Routines
// ----------------------------------------------------------------

// TODO: Shutdown system
void WatchdogISR(void const *n) {
    led3=1; // Activated when the watchdog timer times out
}

void MotorISR(void) {
    osSignalSet(motorId,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt
}

void NavigationISR(void) {
    osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt
}
    
// TODO: Shutdown system
void CollisionISR(void) {
    led4 = 1; // Activated on collision
}