File content as of revision 1:3a40c918ff41:

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


// C.P. Diduch
// EE4333 Robotics Lab-3 
// Implementation of a PI Speed Control System
// December 17, 2012.

#define Dummy 0


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

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

// IO Port Configuration
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut dirL(p22);

Serial BluetoothSerial(p28, p27); // (tx, rx) for PC serial channel
Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel

// Prototypes
void PwmSetOut(float d, float T);
void ReadEncoder();
void InitializeEncoder();

Ticker PeriodicInt;                 
SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
DigitalOut SpiReset(p11); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
DigitalOut SpiStart(p12); // Places SPI interace on the DE0 FPGA into control mode
DigitalOut dir1(p22);

// ******** Main Thread ********
int main() {
char x;
char string[30];

//float d = 0.1;
//float T = 0.001;

led3=0;
led4=0;  

InterruptIn Bumper(p8);  // External interrupt pin
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);

pc.printf("\r\n RTOS Template: \r\n");    
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
PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
do {

    /*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;
            }
            if (u1 > 0)
            {
                dirL = 1;
            }            
            else
            {
                dirL = 0;
            }
    }
        
    // Display variables at the terminal emulator for logging:
    //pc.printf("\r\n%6d %6d %6d %6d %6d %6d", ... );
    Thread::wait(500); // Wait 500 ms
}
while(1);
}

// ******** Control Thread ********
void PiControlThread(void const *argument) {

while (true) {
    osSignalWait(SignalPi, osWaitForever); 
    led2= !led2; // Alive status
    
    float T;
    float d;
    T = 0.001;
    d = abs(u1);
    /*if (u1 < 0)
    {
        dir1 = 1;
    }
    else
    {
        dir1 = 0;
    }
    */
    PwmSetOut(d, T);
   } 
 }

// ******** Collision Thread ********
void ExtCollisionThread(void const *argument) {
while (true) {
    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);
}

//
void PwmSetOut(float d, float T)
{
    PwmOut PwmP21(p21);
    float onTime = d * T; 

    PwmP21.period(T);
    PwmP21.pulsewidth(onTime);
}

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