File content as of revision 15:caa5a93a31d7:

#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);
void UpdateGains(char key, float *increment);
void ShutDown(void);

// Interrupt and thread control
osTimerId oneShotId;
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 x, height;
float speed, steering;

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

    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
    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", deSpi.write(0x8004)); // Read count & time for both motors
}

void AutoTrack(void) {
    char key;
    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
    sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
    
    while (1) {
        osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
        if (pc.readable()) {
            key = pc.getc();
            if (key == 'q') {
                 ShutDown();
                 return;
            }
        }
        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\r\n\r\n", leftMotor, rightMotor);
        Thread::wait(500); // Go to sleep for 500 ms
    }
}

void ManualControl(void) {
    char key;
    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
    while (1) {
        osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
        if (pc.readable()) {
            key = flushBuffer();
            if (key == 'q') ShutDown();
            
            if (key == 'w')  {
                speed++;
            } else if (key == 's') {
                speed--;
            } else if (speed != 0) {
                speed = (speed < 0) ? speed++ : speed--;   
            }
            
            if (key == 'a')  {
                steering++;
            } else if (key == 'd') {
                steering--;
            } else if (steering != 0) {
                steering = (steering < 0) ? steering++ : steering--;   
            }
        } else if (speed != 0) { 
            speed = (speed < 0) ? speed++ : speed--;  
        } else if (steering != 0) {
            steering = (steering < 0) ? steering++ : steering--;  
        }
        
        if (speed > SPEED_MAX) speed = SPEED_MAX ;
        else if (speed < -SPEED_MAX) speed = -SPEED_MAX ;
        
        if (steering > SPEED_MAX) steering = SPEED_MAX ;
        else if (steering < -SPEED_MAX) steering = -SPEED_MAX ;
        
        osMutexWait(motorMutex, osWaitForever); 
        leftMotor = speed - steering;
        rightMotor = speed + steering;
        osMutexRelease(motorMutex); 
        
        Thread::wait(10); // Go to sleep for 100 ms
    }
}

void Tunning(void) {
    char key;
    float increment = 1;
    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
    sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
    
    while (1) {
        osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
        if (pc.readable()) {
            key = pc.getc();
            if (key == 'q') {
                 ShutDown();
                 return;
            }
            UpdateGains(key, &increment);  
            pc.printf("Increment \t %f \r\n", increment);
            pc.printf("Motor     \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI);
            pc.printf("Steering  \t KP: %f, KI: %f \r\n", xPI.kP, xPI.kI);
            pc.printf("Speed     \t KP: %f, KI: %f \r\n\r\n\r\n", heightPI.kP, heightPI.kI); 
        }
        Thread::wait(500); // Go to sleep for 500 ms
    }
}

void ShutDown(void) {
    motorPeriodicInt.detach();
    sensorPeriodicInt.detach();
    osThreadTerminate(navigationId);
    osThreadTerminate(motorId);
    leftPwm.pulsewidth(0);
    rightPwm.pulsewidth(0);
}


// Threads
// ----------------------------------------------------------------
void NavigationThread(void const *argument) {
    int count;
    int missingCount;
    
    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)) {
            missingCount = 0;
            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); 
            if (speed >= 0) {
                leftMotor = speed - steering;
                rightMotor = speed + steering;
            } else {
                leftMotor = speed - steering;
                rightMotor = speed + steering;
            }
            osMutexRelease(motorMutex); 
        } else {
            // When the robot can't see the target spin in last known direction
            if (missingCount >= 30) {
                osMutexWait(motorMutex, osWaitForever);
                leftMotor = (leftMotor > rightMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
                rightMotor = (rightMotor > leftMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
                osMutexRelease(motorMutex); 
            } else {
                missingCount++;    
            }
        }
    } 
}

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
    
}

// Nasty case function to update gain values
void UpdateGains(char key, float *increment) {
    switch(key) {
        case 'e':
            leftMotorPI.kP = leftMotorPI.kP + *increment;
            break;
        case 'd':
            leftMotorPI.kP = leftMotorPI.kP - *increment;
            break;
        case 'r':
            leftMotorPI.kI = leftMotorPI.kI + *increment;
            break;
        case 'f':
            leftMotorPI.kI = leftMotorPI.kI - *increment;
            break;
        case 't':
            xPI.kP = xPI.kP + *increment;
            break;
        case 'g':
            xPI.kP = xPI.kP - *increment;
            break;
        case 'y':
            xPI.kI = xPI.kI + *increment;
            break;
        case 'h':
            xPI.kI = xPI.kI - *increment;
            break;
        case 'u':
            heightPI.kP = heightPI.kP + *increment;
            break;
        case 'j':
            heightPI.kP = heightPI.kP - *increment;
            break;
        case 'i':
            heightPI.kI = heightPI.kI + *increment;
            break;
        case 'k':
            heightPI.kI = heightPI.kI - *increment;
            break;
        case 'o':
            *increment = *increment * 10;
            break;
        case 'l':
            *increment = *increment / 10;
            break; 
    }
}