File content as of revision 5:7108ac9e8182:

// 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
// 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
#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 ReadEncoder();
void SetLeftMotorSpeed(float u);
void SetRightMotorSpeed(float u);
Mutex Var_Lock;

// Global variables for interrupt handler
float u1 = 0; 
float u2 = 0;
float userSetL = 0;
float userSetR = 0;
float prevu1, prevu2;
int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
int startup = 0;
float aeL = 0; 
float aeR = 0;
float eL = 0;
float eR = 0;
float fbSpeedL;
float fbSpeedR;

// --- 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
                19|-|22 Dir, Right Motor, M2
                20|-|21 PWM, Right Motor, M2
*/

// --- IO Port Configuration
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut dirL(p22);
DigitalOut brakeL(p23);
PwmOut PwmL(p21);
DigitalOut dirR(p25);
DigitalOut brakeR(p26);
PwmOut PwmR(p24);
Serial BluetoothSerial(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

Ticker PeriodicInt;                 


// ******** Main Thread ********
int main() 
{    
    InitializeSystem();
    
    pc.printf("\r\n --- Robot Initialization Complete --- \r\n");    
    
    BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
    
    char c;
    
    while(1)
    {
        Var_Lock.lock();
        pc.printf("Left Position: %d \n\r", dPositionLeft);
        pc.printf("Left Time: %d \n\r", dTimeLeft);
        pc.printf("Right Position: %d \n\r", dPositionRight);
        pc.printf("Right Time: %d \n\r", dTimeRight);
        pc.printf("Feedback Speed Left: %f \n\r", fbSpeedL);
        pc.printf("Feedback Speed Right: %f \n\r", fbSpeedR);
        pc.printf("Left u: %f Right u: %f\r\n", u1, u2);
        pc.printf("Left e: %f Right e: %f\r\n", eL, eR);
        pc.printf("Left Ae: %f Right Ae: %f\n\r\n", aeL, aeR);
        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()) 
        {
            pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
            pc.scanf("%f", &userSetL);
            pc.printf("%f", userSetL);
            userSetR = userSetL;
            
            /* x=pc.getc();
            if(x=='w')
            {
                // increase motor speed
                u1 += 0.02;
                if (u1 > 1)
                {
                    u1 = 1;
                }
            }
            else if(x=='s')
            {
                // u1ecrease motor speed
                u1 -= 0.02;
                if (u1 < 0)
                {
                    u1 = 0;
                }
            }
            //else if(x=='a') ...
            //else if(x=='d') ... 
            */
        }
            
        Thread::wait(2000); // Wait 2 seconds
        

    }
}

// ******** Control Thread ********
void PiControlThread(void const *argument) 
{
    while (1) 
    {
        osSignalWait(SignalPi, osWaitForever); 
        led2= !led2; // Alive status
        
        // Read encoder and display results
        ReadEncoder();
        
        //float fbSpeedL;
        //float fbSpeedR;
        //float eL = 0;
        //float eR = 0;
        float maxError = 1000;
        float maxAcc = 10000;
        float Kp = 1.2;
        float Ki = 1.2;
        float leftPos = (float)dPositionLeft;
        float rightPos = (float)dPositionRight;
        float leftMaxPos = 1438.0f;
        float rightMaxPos = 1484.0f;
        
        prevu1 = u1;
        prevu2 = u2;
        
        // calculate feedback speed percentage
        // ****** TODO : BOUND FEEDABCK SPEED
        fbSpeedL = (leftPos/leftMaxPos); 
        fbSpeedR = (rightPos/rightMaxPos);
        
        // 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;
        
        
        // 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.
        */
        
        /*
        pc.printf("Feedback Speed Left: %f \n\r", fbSpeedL);
        pc.printf("Feedback Speed Right: %f \n\r\n", fbSpeedR);
        
        u1 = userSetL;
        u2 = u1;
        */
        
        SetLeftMotorSpeed(u1);
        SetRightMotorSpeed(u2);
    } 
}

// ******** 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
    
    InitializeEncoder();
}

void InitializePWM()
{

}

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.period(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.period(T);
    PwmR.pulsewidth(onTime);
    
    if (u > 0)
    {
        dirR = 1;
    }            
    else
    {
        dirR = 0;
    }
}

void ReadEncoder()
{
    //int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
    
    
    // May be executed in a loop
    
    SpiStart = 1;
    wait_us(5);
    SpiStart = 0;
    DE0.write(0x8004);
    
    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();
    
    // check for bad values
    /*
    if (startup >= 10)
    {
        if (dTimeRight > (EncoderTime + 5) || dTimeRight < (EncoderTime - 5) || dTimeLeft > (EncoderTime + 5) || dTimeLeft < (EncoderTime - 5))
        {
            // Failure!!
            u1 = 0;
            pc.printf("DEO FAILURE!! \n\r\n");
        }
    }
    else
    {
        startup += 1;
    }
    */
    
    /*pc.printf("Left Position: %d \n\r", dPositionLeft);
    pc.printf("Left Time: %d \n\r", dTimeLeft);
    pc.printf("Right Position: %d \n\r", dPositionRight);
    pc.printf("Right Time: %d \n\n\r", dTimeRight);*/
    
    // simply write out the results for now
}