File content as of revision 10:a3ecedc8d9d7:

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


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

// --- 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.05;
#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 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[100]; // Record Feedback Speed
float RecU[100]; // Record Motor Input
float RecX[100]; // Record Integrator Output
float RecE[100]; // Record Error
int RecCount = 0; // Record Counter
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|-|

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 Bumper(p10);  // External interrupt pin
AnalogIn IRFront(p19); // Front IR Ranger Input
AnalogIn IRRear(p20); // Rear IR Ranger Input
Ticker PeriodicInt;                 


// ******** Main Thread ********
int main() 
{    
    InitializeSystem();
    BTInit();
    BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");    
    
    BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
    char x;
    while(1)
    {
        Thread::wait(20);
    
        if (BtS.readable())
        {
            x = BtS.getc();
            
            switch(x)
            {
                case('w'):
                    userSetL = userSetL + 0.05;
                    userSetR = userSetR + 0.05;
                    break;
                case('s'):
                    userSetL = userSetL - 0.05;
                    userSetR = userSetR - 0.05;
                    break;
                case('a'):
                    userSetL = userSetL - 0.025;
                    userSetR = userSetR + 0.025;
                    break;
                case('d'):
                    userSetL = userSetL + 0.025;
                    userSetR = userSetR - 0.025;
                    break;
                case('b'):
                    // Eventually ramp down to 0, RampDown();
                    userSetL = 0;
                    userSetR = 0;
                    break;                
            }
            
        }
        
        /*
        char c;
    
        //Var_Lock.lock();
        pc.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
        pc.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
        pc.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
        pc.printf("e    L: %f R: %f \r\n", eL, eR);
        pc.printf("Ae   L: %f R: %f \n\r", aeL, aeR);
        pc.printf("cSP  L: %f R: %f \r\n\n", cSetL, cSetR);
        //Var_Lock.unlock();
        
        if (pc.readable()){
            x=pc.getc();
            pc.putc(x); //Echo keyboard entry
            osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
               
            } 
        if(pc.readable()) 
        {
            c = pc.getc();
            if (c == 'r')
            {
                userSetL = 0.2;
            }
            if (c == 'p')
            {
                if (RecCount == 100)
                {
                    pc.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
                    int i = 0;
                    for (i = 0; i < 100; i++)
                    {
                        pc.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
                    }
                }    
            
            }
            
            if (c == 'n')
            {
                pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
                pc.scanf("%f", &userSetL);
                
                pc.printf("%f", userSetL);
                userSetR = userSetL;
            }    
        }
            
        Thread::wait(2000);   Wait 2 seconds */
    }
}

// ******** Control Thread ********
void PiControlThread(void const *argument) 
{
    while (1) 
    {
        osSignalWait(SignalPi, osWaitForever); 
        led2= !led2; // Alive status
        
        float prevu1, prevu2;
        //float eL = 0;
        //float eR = 0;
        const unsigned short maxError = 1000;
        const unsigned short maxAcc = 10000;
        // Kp = 0.1, Ki = 0.5
        const float Kp = 0.1f;
        const float Ki = 0.5f;
        
        prevu1 = u1;
        prevu2 = u2;
        
        // read encoder and calculate speed of both motors
        GetSpeeds();
        
        // calculate error
        eL = userSetL - fbSpeedL;
        eR = userSetR - fbSpeedR;
        //eL = -eL;
        //eR = -eR;
        
        // prevent overflow / bound the error
        /*
        if (eL > maxError)
        {
            eL = maxError;
        }
        if (eL < -maxError);
        {
            eL = -maxError;
        }
        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;
        
        cSetL = userSetL + u1;
        cSetR = userSetR + u2;
        
        //u1 = -u1;
        //u2 = -u2;
        // Is signaled by a periodic timer interrupt handler
        /*
        Read incremental position, dPosition, and time interval from the QEI.
        e = Setpoint – dPosition // e is the velocity error
        xState = xState + e;    // x is the Euler approximation to the integral of e.
        u = Kp*e + Ki*xState;       // u is the control signal
        Update PWM on-time register with abs(u);
        Update the DIR pin on the LMD18200 with the sign of u.
        */
        if (userSetL == 0.2f)
        {
            if (RecCount < 100)
            {
                RecX[RecCount] = aeL;
                RecU[RecCount] = cSetL;
                RecV[RecCount] = fbSpeedL;
                RecE[RecCount] = eL;
                RecCount++;
            }
            else
            {
                userSetL = 0;
            }
           
        }
        SetLeftMotorSpeed(cSetL);
        SetRightMotorSpeed(cSetR);
    } 
}

// ******** Collision Thread ********
void ExtCollisionThread(void const *argument) 
{
    while (1) 
    {
        osSignalWait(SignalExtCollision, osWaitForever);
        led4 = !led4;
    }
}

// ******** Watchdog Interrupt Handler ********
void Watchdog(void const *n) 
{
    led3=1;
    pc.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; 
    
    Bumper.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();
}

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


// --- Other Functions
void SetLeftMotorSpeed(float u)
{
    float T;
    float d;
    float onTime;
    
    // bound the input
    if (u > 1)
    {
        u = 1;
    }
                
    if (u < -1)
    {
        u = -1;
    }
    
    // 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;
    
    // bound the input
    if (u > 1)
    {
        u = 1;
    }
                
    if (u < -1)
    {
        u = -1;
    }
    
    // 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()
{
    float leftMaxPos = 1480.0f;
    float rightMaxPos = 1480.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
    
    // figure out which direction the motor is turning
    /*
    if (dPositionLeft > 32767)
    {
        // turning backwards
        *leftSpeed = -(65535 - dPositionLeft)/leftMaxPos;
    }
    else
    {
        // turning forwards
        *leftSpeed = dPositionLeft/leftMaxPos;
    }
    
    if (dPositionRight > 32767)
    {
        // turning backwards
        *rightSpeed = -(65535 - dPositionRight)/rightMaxPos;
    }
    else
    {
        // turning forwards
        *rightSpeed = dPositionRight/rightMaxPos;
    }
    */
    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;
    // Read Sensors
    IRF = IRFront.read();
    IRR = IRRear.read();
    // Voltage Corresponding to nominal distance
    float Nominal = 3.0;
    // Variable for turning robots
    float Turn = 0.1;
    
    // Ensure Robot isn't closer than 10cm from wall (reads no voltage)
    
    // Compare to nominal voltage and adjust
    if (IRF > Nominal && IRR > Nominal)
    {
        SetR = SetR - Turn;
        SetL = SetL + Turn;
        SetRightMotorSpeed(SetR);
        SetLeftMotorSpeed(SetL);
    }
    if (IRF < Nominal && IRR < Nominal
    {
        SetR = SetR - Turn;
        SetL = SetL + Turn;
        SetRightMotorSpeed(SetR);
        SetLeftMotorSpeed(SetL);
    }
    
    // Compare rangers to each other
    if (IRF > IRR) // IRF closer than IRR
    {
        SetR = SetR + Turn;
        SetL = SetL - Turn;
        SetRightMotorSpeed(SetR);
        SetLeftMotorSpeed(SetL);
    }
    if (IRF < IRR) // IRF further than IRR
    {
        SetR = SetR - Turn;
        SetL = SetL + Turn;
        SetRightMotorSpeed(SetR);
        SetLeftMotorSpeed(SetL);
    }
   
} */

void BTInit()
{
            BtS.printf("AT+BTCANCEL\r\n");
            BtS.printf("AT+BTSCAN\r\n");
}