File content as of revision 8:b0478286ad21:

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

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

// Function prototypes
// ----------------------------------------------------------------
void MotorISR(void);
void NavigationISR(void);
void CollisionISR(void);
void WatchdogISR(void const *n);
void MotorThread(void const *argument);
void NavigationThread(void const *argument);

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

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

// Set points and display variables
float leftMotor, rightMotor;
int x, height;
float speed, steering;
bool isInitialized;

// Functions
// ----------------------------------------------------------------
void AutoTrack() { 
    RunMotor();
    RunNavigation();
    
    while (1) {
        
        ResetWatchDog();

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

void RunMotor(void) {
    // TODO: Optimize interrupt time for motor... Currently too fast
    InitializeRobot();
    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
}

void RunNavigation(void) {
    InitializeRobot();
    navigationPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
}

void ManualControl(void) {

}

// Setup the robot threads and data if needed
void InitializeRobot(void) {
    if (isInitialized) return;
    
    motorMutex = osMutexCreate(osMutex(motorMutex));
    
    bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
    
    // Create 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 \r\n\r\n", deSpi.write(0x8004)); // Read count & time for both motors
    
    isInitialized = true;
}

// Timesout in 2 seconds
void ResetWatchDog(void) {
    osTimerStart(oneShotId, 2000);
}


// 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)) {
            height = pixy.blocks[0].height; //use this for now
            x = pixy.blocks[0].x;
            
            speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX);
            steering = xPI.Run(X_SETPOINT-x, SPEED_MAX);

            // Give setpoints to MotorThread
            osMutexWait(motorMutex, osWaitForever); 
            leftMotor = speed - steering;
            rightMotor = speed + steering;
            osMutexRelease(motorMutex); 
        }
    } 
}

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

    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);
        timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD);
        
        // Run PI control on right motor
        dP = deSpi.write(0);
        dt = deSpi.write(0); 
        angVel = -QE2RadsPerSec(dP, dt); // motors have opposite orientations
        timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD); //
        
        // Output new PWM and direction 
        if (timeOnLeft >= 0) leftDir = 1;   
        else leftDir = 0;
        
        if (timeOnRight >= 0) rightDir = 0;   
        else rightDir = 1;
        
        leftPwm.pulsewidth(fabs(timeOnLeft));
        rightPwm.pulsewidth(fabs(timeOnRight));
    } 
}


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