File content as of revision 19:05b8123905fb:

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

enum System {
    MOTOR = '0', 
    SPEED = '1', 
    STEERING = '2'
};

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

// Variables
// ----------------------------------------------------------------

// For tunning and response measurement
bool isTunning;
int motorSample;
int navigationSample;
float leftMotorResponse[MOTOR_SAMPLES];
float rightMotorResponse[MOTOR_SAMPLES];
float speedResponse[NAVIGATION_SAMPLES];
float steeringResponse[NAVIGATION_SAMPLES];


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


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

    motorMutex = osMutexCreate(osMutex(motorMutex));
    
    // 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
    deSpi.frequency(500000);
    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
    FlushBuffer();
}

// Setup robot for auto tracking and listen for commands
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;
            } else if (key == 'm') {
                ShutDown();
                ManualControl();
            } 
        }
        
        pc.printf("X: %d, Height: %d \r\n", x, height);
        pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering);

        Thread::wait(250);
    }
}

// Setup robot for manual control and listen for commands
void ManualControl(void) {
    char key;
    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
    float speedRate, steeringRate;
    
    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;
            } else if (key == 'a') {
                ShutDown();
                AutoTrack();
            } else if (key == '8') {
                speedRate = (speedRate < 1)? speedRate + 0.1 : 1;
            } else if (key == '5') {
                speedRate = (speedRate > -1)? speedRate - 0.1 : -1;
            } else if (key == '6') {
                steeringRate = (steeringRate < 1)? steeringRate + 0.1 : 1;
            } else if (key == '4') {
                steeringRate = (steeringRate > -1)? steeringRate - 0.1 : -1;
            } 
            
            pc.printf("%.2f,%.2f\r\n", speedRate, steeringRate); 
        }
        
        osMutexWait(motorMutex, osWaitForever);  
        leftMotor = SPEED_MAX*(speedRate + steeringRate);
        rightMotor = SPEED_MAX*(speedRate - steeringRate);
        osMutexRelease(motorMutex); 
    }
}

void Tunning(void) {
    System system;
    int i;
    char key;
    float increment = 1;
    
    isTunning = true;
    
    pc.printf("\e[1;1H\e[2J");
    pc.printf("Motor (0), Speed (1), Steering (2), Print (p) \r\n");
    pc.printf("=> ");
    
    while (1) {
        osTimerStart(oneShotId, 20000); // Start or restart the watchdog timer interrupt and set to  20000ms.
        if (pc.readable()) {
            key = pc.getc();
            if ((key=='0')||(key=='1')||(key=='2')) {
                system = (System)key;
            } else if (key == 'q') {
                ShutDown();
                return;
            } else if (key == 'p') {
                
                pc.printf("\r\n\r\n  Left Motor \t Right Motor \t Speed   \t Steering \r\n");
                
                while ((i < MOTOR_SAMPLES)||(i < NAVIGATION_SAMPLES)) {
                    if (i < MOTOR_SAMPLES) {
                        pc.printf("  %f, \t %f, \t ", 
                            rightMotorResponse[i],
                            leftMotorResponse[i]
                        );
                    } else pc.printf("           \t             \t");
                    
                    if (i < NAVIGATION_SAMPLES) {
                        pc.printf("%f, \t %f ", 
                            speedResponse[i],
                            steeringResponse[i]
                        );
                    } else pc.printf("           \t   ");
                    pc.printf("\r\n");
                    i++;
                }
                
            } else {
                switch (system) {
                    case MOTOR:
                        if (key == 'a') leftMotorPI.kP = leftMotorPI.kP + increment;   
                        else if (key == 'z') leftMotorPI.kP = leftMotorPI.kP - increment; 
                        else if (key == 's') leftMotorPI.kI = leftMotorPI.kI + increment; 
                        else if (key == 'x') leftMotorPI.kI = leftMotorPI.kI - increment; 
                        break;
                    case SPEED:
                        if (key == 'a') heightPI.kP = heightPI.kP + increment;   
                        else if (key == 'z') heightPI.kP = heightPI.kP - increment; 
                        else if (key == 's') heightPI.kI = heightPI.kI + increment; 
                        else if (key == 'x') heightPI.kI = heightPI.kI - increment; 
                        break;
                   case STEERING:
                        if (key == 'a') xPI.kP = xPI.kP + increment;   
                        else if (key == 'z') xPI.kP = xPI.kP - increment; 
                        else if (key == 's') xPI.kI = xPI.kI + increment; 
                        else if (key == 'x') xPI.kI = xPI.kI - increment; 
                        break;
                }
                
                if (key == 'd') increment = increment*10;   
                else if (key == 'c') increment = increment/10; 
            }
            
            pc.printf("\e[1;1H\e[2J");
            pc.printf("Current system: %c \r\n\r\n", system);
            
            pc.printf("Motor (0) \t Speed (1) \t Steering (2) \t Print (p) \r\n");
            pc.printf("Kp (a/z)  \t Ki (s/x)  \t inc (d/c) \r\n\r\n");
            
            pc.printf("Increment \t %f \r\n", increment);
            pc.printf("Motor     \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI);
            pc.printf("Speed     \t KP: %f, KI: %f \r\n", heightPI.kP, heightPI.kI); 
            pc.printf("Steering  \t KP: %f, KI: %f \r\n\r\n", xPI.kP, xPI.kI);
            pc.printf("=> ");
            
            if (key != 'p') {
                motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
                sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
                motorSample = 0;
                navigationSample = 0;
                i = 0;
            }
        }
        
        // Stop when all samples are collected
        if ((navigationSample >= NAVIGATION_SAMPLES)&&(motorSample >= MOTOR_SAMPLES)) {
            motorPeriodicInt.detach();
            sensorPeriodicInt.detach();   
            leftPwm.pulsewidth(0);
            rightPwm.pulsewidth(0);
            xPI.Reset();
            heightPI.Reset();
            leftMotorPI.Reset();
            rightMotorPI.Reset();
        }
        Thread::wait(500); // Go to sleep for 500 ms
    }
}

void ShutDown(void) {
    motorPeriodicInt.detach();
    sensorPeriodicInt.detach();
    leftPwm.pulsewidth(0);
    rightPwm.pulsewidth(0);
    isTunning = false;
    motorSample = 0;
    navigationSample = 0;
    xPI.Reset();
    heightPI.Reset();
    leftMotorPI.Reset();
    rightMotorPI.Reset();
    pc.printf("Robot Shutdown... \r\n\r\n");
    FlushBuffer();
}

// 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;
            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); 
        } 
        
        if (isTunning && (navigationSample < NAVIGATION_SAMPLES)) {
            speedResponse[navigationSample] = speed; 
            steeringResponse[navigationSample] = steering;
            navigationSample++;
        }
    } 
}

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

    while (1) {
        osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
        
        // Get setpoints from navigation
        osMutexWait(motorMutex, osWaitForever); 
        leftSet = leftMotor;
        rightSet = rightMotor;
        osMutexRelease(motorMutex); 
        
        // Run PI control on right motor
        dP = deSpi.write(0);
        dt = deSpi.write(0); 
        angVel = RadsPerSec(dP, dt); // motors have opposite orientations
        timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD/2);
        
        // Run PI control on left motor
        dP = deSpi.write(0);
        dt = deSpi.write(0); 
        angVel = -RadsPerSec(dP, dt);
        timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD/2);
        
        
        // Output new PWM and direction 
        if (timeOnLeft >= 0) leftDir = 0;   
        else leftDir = 1;
        
        if (timeOnRight >= 0) rightDir = 0;   
        else rightDir = 1;
        
        leftPwm.pulsewidth(fabs(timeOnLeft));
        rightPwm.pulsewidth(fabs(timeOnRight));
        
        if (isTunning && (motorSample < MOTOR_SAMPLES)) {
            leftMotorResponse[motorSample] = timeOnLeft; 
            rightMotorResponse[motorSample] = timeOnRight;
            motorSample++;
        } 
        
        #ifdef DEBUG
        loop = (loop < 1/MOTOR_PERIOD)? loop + 1 : 0;       
        #endif
    } 
}


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

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

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
}