File content as of revision 24:f4f933d9194b:

// Robot Control Code
// Tom Elliott and Ian Colwell

/* TODO on a sunny day
- reduce the amount of global variables
- try increasing bluetooth buad rate
*/

#include "mbed.h"
#include "rtos.h"

extern "C" void mbed_reset();

// --- Constants
#define Dummy 0

//#define PWMPeriod 0.0005 // orignally 0.001 (gave a high pitched whine)
const float PWMPeriod = 0.00005;
// Control Update = 50ms (time should be 609/610) (if we want to change this we also have to change the way feedback speed is calculated)
//#define ControlUpdate 0.05
const float ControlUpdate = 0.04;
#define EncoderTime 610


// --- Function prototypes
void PiControllerISR(void);
void WdtFaultISR(void);
void ExtCollisionISR(void);
void PiControlThread(void const *argument);
void ExtCollisionThread(void const *argument);
void Watchdog(void const *n);
void InitializeSystem();
void InitializeEncoder();
void InitializePWM();
void PwmSetOut(float d, float T);
void GetSpeeds();
void SetLeftMotorSpeed(float u);
void SetRightMotorSpeed(float u);
void DisplayMenu();
void Ramp(float speed, unsigned int time, unsigned short motor);
void IRChecker();
void BTInit();

// Global variables
float u1 = 0; 
float u2 = 0;
float cSetL = 0;
float cSetR = 0;
float userSetL = 0;
float userSetR = 0;
int startup = 0;
float aeL = 0; 
float aeR = 0;
float RecV[200]; // Record Feedback Speed
float RecU[200]; // Record Motor Input
float RecX[200]; // Record Integrator Output
float RecE[200]; // Record Error
int RecCount = 0; // Record Counter
unsigned short action;
float DF = 0;
float DR = 0;
float TSpeedL = 0;
float TSpeedR = 0;
float Turn = 0.1;
float aeW = 0;
float eW = 0;
float uW = 0; 
float prevuW = 0;

float Nominal = 35;
float Kpos = 0.01;
float Kor = 0.01;
float KpW = 0.3;
float KiW = 0.0;
int autoSpeed = -1;

Mutex Var_Lock;

// global variables to eventually be removed
short dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
float fbSpeedL, fbSpeedR;
float eL = 0;
float eR = 0;

// --- Processes and threads
int32_t SignalPi, SignalWdt, SignalExtCollision;
osThreadId PiControl,WdtFault,ExtCollision;
osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
osTimerDef(Wdtimer, Watchdog);

/* PIN-OUT

MOSI Quad Enc    5|-|
MISO Quad Enc    6|-|
SCK Quad Enc     7|-|
SPI Start Quad E 8|-|
SPI Reset Quad E 9|-|
Emergency Stop  10|-|
Bluetooth tx    13|-|28
Bluetooth rx    14|-|27
                15|-|26 Brake, Left Motor, M1
                16|-|25 Dir, Left Motor, M1
                17|-|24 PWM, Left Motor, M1
                18|-|23 Brake, Right Motor, M2
Front IR        19|-|22 Dir, Right Motor, M2
Rear  IR        20|-|21 PWM, Right Motor, M2
*/

// --- IO Port Configuration
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut dirR(p22);
DigitalOut brakeR(p23);
PwmOut PwmR(p21);
DigitalOut dirL(p25);
DigitalOut brakeL(p26);
PwmOut PwmL(p24);
Serial BtS(p13, p14); // (tx, rx) for PC serial channel
Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
//InterruptIn EmerStop(p10);  // External interrupt pin
AnalogIn IRFront(p19); // Front IR Ranger Input
AnalogIn IRRear(p20); // Rear IR Ranger Input
Ticker PeriodicInt;                 
DigitalIn EmergencyStop(p10);

// ******** Main Thread ********
int main() 
{    
    char c;
    
    InitializeSystem();
    BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");  
    pc.printf("\r\n --- Robot Initialization Complete --- \r\n"); 
    DisplayMenu();
    
    //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
    while(1)
    {        
        if(BtS.readable()) 
        {
            c = BtS.getc();
            
            // quit
            if (c == 'q')
            {
                action = 0;
                // erase errors
                aeW = 0;
                eW = 0;
                aeL = 0;
                aeR = 0;
                Ramp(0, 800, 0);
                DisplayMenu();
                continue;
            }
            
            if (action == 0)
            {
                // choose a menu selection
                switch(c)
                {
                    case 'd':
                        action = 1;
                        BtS.printf("\n\rTap w-a-s-d keys for differential speed control\r\nPress 'q' to quit \r\n");
                        break;
                    case 'w':
                        BtS.printf("\n\r Welcome to Wall Following, use w-s to control robot speed");
                        BtS.printf("\n\r Use a-d to increase/decrease turning radius");
                        BtS.printf("\n\r Use < and > to increase/decrease wall distance");
                        userSetL = 0.2;
                        userSetR = 0.2;
                        TSpeedL = 0.2;
                        TSpeedR = 0.2;
                        action = 2;
                        break;
                    case '0':
                        action = 3;
                        break;
                    case 'r':
                        action = 4;
                        userSetL = 0.2;
                        userSetR = 0.2;
                        break;
                    default:
                        BtS.printf("\n\r Command not recognized \n\r");
                        action = 0;
                        break;
                }
                continue;
            }
            
            if (action == 1)
            {
                // keyboard input to drive robot using wasd 
                switch(c)
                {
                    case('w'):
                        userSetL = userSetL + 0.1;
                        userSetR = userSetR + 0.1;
                        break;
                    case('s'):
                        userSetL = userSetL - 0.1;
                        userSetR = userSetR - 0.1;
                        break;
                    case('a'):
                        userSetL = userSetL - 0.04;
                        userSetR = userSetR + 0.04;
                        break;
                    case('d'):
                        userSetL = userSetL + 0.04;
                        userSetR = userSetR - 0.04;
                        break;          
                    case('e'):
                        Ramp(0.5, 500, 0);
                        break;
                    case('r'):
                        Ramp(0, 500, 0);
                        break;
                }
                if (userSetL > 0.5)
                {
                    userSetL = 0.5;
                }
                if (userSetL < -0.5)
                {
                    userSetL = -0.5;
                }
                if (userSetR > 0.5)
                {
                    userSetR = 0.5;
                }
                if (userSetR < -0.5)
                {
                    userSetR = -0.5;
                }
                
                continue;
            }
            
            if (action == 2)
            {
                // keyboard input to wall following
                switch(c)
                {
                    case('w'):
                        TSpeedL = TSpeedL + 0.05;
                        TSpeedR = TSpeedR + 0.05;
                        break;
                    case('s'):
                        TSpeedL = TSpeedL - 0.05;
                        TSpeedR = TSpeedR - 0.05;
                        break;
                    case('a'):
                        Turn = Turn + 0.01;
                        break;
                    case('d'):
                        Turn = Turn - 0.01;
                        break;
                    case(','):
                        Nominal = Nominal - 2.5;
                        break;
                    case('.'):
                        Nominal = Nominal + 2.5;
                        break; 
                    case('g'):
                        autoSpeed = autoSpeed * -1;
                        break;
                    case('n'):
                        BtS.printf("\n\r Current constants: Ki %.3f:, Kp: %.3f, Kor: %.3f, Kpos: %.3f \n\r Select the constant you wish to change:", KiW, KpW, Kor, Kpos);    
                        char k;
                        float newConst;
                        while (1)
                        {
                            if (BtS.readable())
                            {
                                k = BtS.getc();
                                if (k == '1')
                                {
                                    BtS.scanf("%f", &newConst);
                                    KiW = newConst;
                                    break;
                                }
                                if (k == '2')
                                {
                                    BtS.scanf("%f", &newConst);
                                    KpW = newConst;
                                    break;
                                }
                                if (k == '3')
                                {
                                    BtS.scanf("%f", &newConst);
                                    Kor = newConst;
                                    break;
                                }
                                if (k == '4')
                                {
                                    BtS.scanf("%f", &newConst);
                                    Kpos = newConst;
                                    break;
                                }
                                
                                printf("\n\r Pick a constant ya goof \n\r");
                            }
                        }
                }
            }
            if (action == 3)
            {
                // keyboard input to debug mode
                float newSpeed;
                BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
                BtS.scanf("%f", &newSpeed);
                
                BtS.printf("%f", newSpeed);
                Ramp(newSpeed, 20, 0);
                //userSetR = userSetL;
            }            
            
            if (action == 2 && c == 'r')
            {
                if (RecCount == 200)
                {
                    BtS.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
                    int i = 0;
                    for (i = 0; i < 200; i++)
                    {
                        BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
                    }
                }
            }     
        }// close if(BtS.readable())
        if (action == 2)
        {
            // Wall Following
            //Var_Lock.lock();
            BtS.printf("IR F: %f cm R: %f cm \n\r", DF, DR);
            BtS.printf("Wall Error: %f \n\r", eW);
            BtS.printf("Acc Error: %f \n\r", aeW);
            BtS.printf("Diff. Setpoint: %f \n\r", uW);
            BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
            //Var_Lock.unlock();
            Thread::wait(1000);
        }
        
        if (action == 3)
        {
            // Debug Mode
            
            float IRF, IRR;
            IRF = IRFront.read();
            IRR = IRRear.read();
            
            Var_Lock.lock();
            BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
            BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
            BtS.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
            BtS.printf("e    L: %f R: %f \r\n", eL, eR);
            BtS.printf("Ae   L: %f R: %f \n\r", aeL, aeR);
            BtS.printf("cSP  L: %f R: %f \n\r", cSetL, cSetR);
            BtS.printf("IR   F: %f R: %f \n\r", IRF, IRR);
            Var_Lock.unlock();
            Thread::wait(2000);
        }          
    }
}

// ******** Control Thread ********
void PiControlThread(void const *argument) 
{
    float maxError = 1.0f;
    float maxAcc = 10.0f;
    
    while (1) 
    {
        // check for emergency stop
        if (EmergencyStop == 1)
        {
        userSetL = 0;
        userSetR = 0;
        SetLeftMotorSpeed(userSetL);
        SetRightMotorSpeed(userSetR);
        BtS.printf("\n\rEmergency Stop!!\n\r");
        Thread::wait(2000);
        }
        
        osSignalWait(SignalPi, osWaitForever); 
        led2= !led2; // Alive status
        
        float prevu1, prevu2;

        // Kp = 0.1, Ki = 0.5
        const float Kp = 0.5f;
        const float Ki = 0.8f;
        
        if (action == 2)
        {
            IRChecker();
        }
        prevu1 = u1;
        prevu2 = u2;
        
        // read encoder and calculate speed of both motors
        GetSpeeds();
        
        // calculate error
        eL = userSetL - fbSpeedL;
        eR = userSetR - fbSpeedR;
        
        /*
        // prevent overflow / bound the error    
        if (eL > 1)
        {
            eL = 1;
        }
        if (eL < -1);
        {
            eL = -1;
        }
        if (eR > maxError)
        {
            eR = maxError;
        }
        if (eR < -maxError);
        {
            eR = -maxError;
        } 
        */
        // accumulated error (integration)
        if (prevu1 < 1 && prevu1 > -1)
        {    
            aeL += eL;
        }
        if (prevu2 < 1 && prevu2 > -1)
        {
            aeR += eR;
        }
        
        /*
        // bound the accumulatd error
        if (aeL > maxAcc)
        {
            aeL = maxAcc;
        }
        if (aeL < -maxAcc)
        {
            aeL = -maxAcc;
        }
        if (aeR > maxAcc)
        {
            aeR = maxAcc;
        }
        if (aeR < -maxAcc)
        {
            aeR = -maxAcc;
        }
        */
        
        u1 = Kp*eL + Ki*aeL;
        u2 = Kp*eR + Ki*aeR;
        
        SetLeftMotorSpeed(u1);
        SetRightMotorSpeed(u2);
    } 
}

// ******** Collision Thread ********
void ExtCollisionThread(void const *argument) 
{
    while (1) 
    {
        osSignalWait(SignalExtCollision, osWaitForever);
        userSetL = 0;
        userSetR = 0;
        SetLeftMotorSpeed(userSetL);
        SetRightMotorSpeed(userSetR);
        mbed_reset();
    }
}

// ******** Watchdog Interrupt Handler ********
void Watchdog(void const *n) 
{
    led3=1;
    BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
}

// ******** Period Timer Interrupt Handler ********
void PiControllerISR(void)
{
    osSignalSet(PiControl,0x1);
}
    
// ******** Collision Interrupt Handler ********
void ExtCollisionISR(void) 
{
    osSignalSet(ExtCollision,0x1);
}


// --- Initialization Functions
void InitializeSystem()
{
    led3=0;
    led4=0; 
    
    //EmerStop.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
    
    // Start execution of the Threads
    PiControl = osThreadCreate(osThread(PiControlThread), NULL);
    ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
    osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
    PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
    
    InitializePWM();
    InitializeEncoder();
    BTInit();
}

void InitializePWM()
{
    PwmL.period(PWMPeriod);
    PwmR.period(PWMPeriod);
}

void InitializeEncoder()
{
    // Initialization – to be executed once (normally)
    DE0.format(16,0);   // SPI format: 16-bit words, mode 0 protocol.
    DE0.frequency(1000000);
    SpiStart = 0;
    SpiReset = 1;
    wait_us(10);
    SpiReset = 0;
    DE0.write(0x8004);  // SPI slave control word to read (only) 4-word transactions
                            // starting at base address 0 within the peripheral.
}

void BTInit()
{
    BtS.printf("AT+BTCANCEL\r\n");
    BtS.printf("AT+BTSCAN\r\n");
    
    // also send to the pc so we know whats going on
    pc.printf("AT+BTCANCEL\r\n");
    pc.printf("AT+BTSCAN\r\n");    
}

// --- Other Functions
void SetLeftMotorSpeed(float u)
{
    float T;
    float d;
    float onTime;
    float maxMotorSpeed = 0.5;
    
    // bound the input
    if (u > maxMotorSpeed)
    {
        u = maxMotorSpeed;
    }
                
    if (u < -maxMotorSpeed)
    {
        u = -maxMotorSpeed;
    }
    
    // calculate duty cycle timing
    T = PWMPeriod;
    d = abs(u);
    onTime = d * T; 

    PwmL.pulsewidth(onTime);
    
    if (u > 0)
    {
        dirL = 1;
    }            
    else
    {
        dirL = 0;
    }
}

void SetRightMotorSpeed(float u)
{
    float T;
    float d;
    float onTime;
    float maxMotorSpeed = 0.5;
    
    // bound the input
    if (u > maxMotorSpeed)
    {
        u = maxMotorSpeed;
    }
                
    if (u < -maxMotorSpeed)
    {
        u = -maxMotorSpeed;
    }
    
    // calculate duty cycle timing
    T = PWMPeriod;
    d = abs(u);
    onTime = d * T; 

    PwmR.pulsewidth(onTime);
    
    if (u > 0)
    {
        dirR = 1;
    }            
    else
    {
        dirR = 0;
    }
}

void GetSpeeds()
{
    // when using a interrupt period of 50ms: 1480
    float leftMaxPos = 1184.0f;
    float rightMaxPos = 1184.0f;
    
    // Restart the SPI module each time
    SpiStart = 1;
    wait_us(5);
    SpiStart = 0;
    DE0.write(0x8004);
    
    // read in 4 16-bit words
    Var_Lock.lock();
    dPositionLeft = DE0.write(Dummy);   // Read QEI-0 position register 
    dTimeLeft = DE0.write(Dummy);       // Read QE-0 time interval register
    dPositionRight = DE0.write(Dummy);  // Read QEI-1 position register 
    dTimeRight = DE0.write(Dummy);      // Read QEI-1 time interval register
    Var_Lock.unlock();
    
    // calcspeed
    fbSpeedL = ((float)dPositionLeft)/leftMaxPos;
    fbSpeedR = ((float)dPositionRight)/rightMaxPos;
    
    // bound the feedback speed
    if (fbSpeedL > 1)
    {
        fbSpeedL = 1;
    }
    if (fbSpeedL < -1)
    {
        fbSpeedL = -1;
    }
    if (fbSpeedR > 1)
    {
        fbSpeedR = 1;
    }
    if (fbSpeedR < -1)
    {
        fbSpeedR = -1;
    }
} 

void IRChecker()
{
    float IRF, IRR, IRF1, IRR1, IRF2, IRR2, prevDF, prevDR;
    float speedSet;
    float aF = 22.6321; // 34.2911
    float bF = -0.1721; // -0.2608
    float aR = 22.6021; // 34.2456
    float bR = -0.0376; // -0.0569
    
    // Read Sensors
    IRF1 = 3.3*IRFront.read();
    IRR1 = 3.3*IRRear.read();
    IRF2 = 3.3*IRFront.read();
    IRR2 = 3.3*IRRear.read();
    
    // average
    IRF = (IRF1 + IRF2)/2;
    IRR = (IRR1 + IRR2)/2;
    
    prevDF = DF;
    prevDR = DR;
    
    // Calculate distance based on voltage
    DF = aF/(IRF+bF);
    DR = aR/(IRR+bR);
    
    prevuW = uW;
    
    // check for invalid data
    if (DF < 0)
    {
        DF = 80;
    }
    if (DR < 0)
    {
        DR = 80;
    }
    if (DF > 80)
    {
        DF = 80;
    }
    if (DR > 80)
    {
        DR = 80;
    }
    if (DF < 10)
    {
        DF = 10;
    }
    if (DR < 10)
    {
        DR = 10;
    }
    
    // calculate errors
    eW = Kpos*(Nominal - (DF + DR)/2) + Kor*(DR - DF);
    
    // accumulate error
    if (prevuW < Turn && prevuW > -Turn)
    {
        aeW = aeW + eW;
    }
    
    uW = KpW*eW + KiW*aeW;
    
    if (uW > Turn)
    {
        uW = Turn;
    }
    else if (uW < -Turn)
    {
        uW = -Turn;
    }
    
    // set speed using auto speed control
    if (autoSpeed == 1)
    {
        speedSet = (1 - (abs(eW)*5))*TSpeedL;
        if (speedSet < 0.05)
        {
            speedSet = 0.05;
        }
        userSetL = speedSet + uW;
        userSetR = speedSet - uW;   
    }
    else
    {
        // set differential speeds
        userSetL = TSpeedL + uW;
        userSetR = TSpeedR - uW;
    }   
    
    // data recording code
    if (action == 2)
    {
        if (RecCount < 200)
        {
            RecX[RecCount] = eW;
            RecU[RecCount] = uW;
            RecV[RecCount] = DF;
            RecE[RecCount] = DR;
            RecCount++;
        }
    }
}

void DisplayMenu()
{
    BtS.printf("\r\n\nPress the corresponding key to do something:\r\n");
    BtS.printf("| Key | Action\n\r");
    BtS.printf("|-----|----------------------------\n\r");
    BtS.printf("|  d  | Drive the robot using wasd keys\n\r");
    BtS.printf("|  w  | Robot performs wall following\n\r");
    BtS.printf("|  0  | Debug mode\n\r");
    BtS.printf("|  r  | Record Data \n\r");
    BtS.printf("|  q  | Quit current action, stop the robot, and return to this menu\n\r\n");   
}

void Ramp(float speed, unsigned int time, unsigned short motor)
{
    const unsigned short steps = 20;
    float changeL = (speed - userSetL)/steps;
    float changeR = (speed - userSetR)/steps;
    unsigned short i;
    // calculate wait time (we wont worry too much about rounding errors)
    unsigned int waitTime = time/steps;
    
    for (i = 0; i < steps; i++)
    {
        //Thread::wait(200);
        Thread::wait(waitTime);
        
        if (motor == 0)
        {
            // change both speeds
            userSetL += changeL;
            userSetR += changeR;
            continue;
        }
        if (motor == 1)
        {
            userSetL += changeL;
            continue;
        }
        if (motor == 2)
        {
            userSetR += changeR;
        }  
    }   
}