File content as of revision 2:1c5cc180812d:

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


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

// --- Constants
#define Dummy 0
#define PWMPeriod 0.001

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

// Global variables for interrupt handler
float u1;
float u2;

// --- 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, Right Motor, M2
                16|-|25 Dir, Right Motor, M2
                17|-|24 PWM, Right Motor, M2
                18|-|23 Brake, Left Motor, M1
                19|-|22 Dir, Left Motor, M1
                20|-|21 PWM, Left Motor, M1
*/

// --- 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: ");
    
    while(1)
    {

        /*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", &u1);
            pc.printf("%f", u1);
            /* 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') ... 
            */
                if (u1 > 1)
                {
                    u1 = 1;
                }
                
                if (u1 < -1)
                {
                    u1 = -1;
                }
        }
            
        Thread::wait(500); // Wait 500 ms
    }
}

// ******** Control Thread ********
void PiControlThread(void const *argument) 
{
    while (1) 
    {
        osSignalWait(SignalPi, osWaitForever); 
        led2= !led2; // Alive status
        
        // Read encoder and display results
        ReadEncoder();
        
        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;
}

// ******** 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, .02); // 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.
    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;
    
    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;
    
    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
    dPositionRight = DE0.write(Dummy); // Read QEI-0 position register 
    dTimeRight = DE0.write(Dummy);     // Read QE-0 time interval register
    dPositionLeft = DE0.write(Dummy);  // Read QEI-1 position register 
    dTimeLeft = DE0.write(Dummy);      // Read QEI-1 time interval register
    
    // simply write out the results for now
    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);
}