File content as of revision 1:f1d9b55f7dd7:

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

// IO Port Configuration
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
PwmOut PwmR(p21); // define p21 as PwmR, p22 as PwmL
PwmOut PwmL(p22); // define p21 as PwmR, p22 as PwmL
DigitalOut DirR(p30); // define p30 as DirR, p29 as DirL
DigitalOut DirL(p29); // define p30 as DirR, p29 as DirL
DigitalOut SpiReset(p11);//Reset for all devices within the slave SPI peripheral in the DE0 FPGA
DigitalOut IoReset(p12); //Places SPI interface on the DE0 FPGA into control mode
SPI DE0(p5,p6,p7); //(mosi,miso,sclk)DE0 is the SPI channel with the FPGA
Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
Serial BluetoothSerial(p9, p10); // tx, rx
Ticker PeriodicInt;      // Declare a timer interrupt: PeriodicInt
InterruptIn Bumper(p8);  // External interrupt pin declared as Bumper

// Function prototypes
void Initialize(void);
char DisplayMenu(void);
void ManualControl(void);
void BluetoothModule(void);
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 PIController(void);
void SpeedControl(void);
int SatAdd(int x, int y);
int SatSub(int x, int y);
int SatValue(int x, int Limit);
int Signextend(int x);
void Userinterface(void);
void UsRangefinder(void);
void RcServoMotor(void);


// Processes and threads
int32_t SignalPi, SignalWdt, SignalExtCollision;
osThreadId PiControl,WdtFault,ExtCollision;
osThreadDef(PiControlThread, osPriorityRealtime, DEFAULT_STACK_SIZE); // Declare Control as a thread/process
osThreadDef(ExtCollisionThread, osPriorityHigh, DEFAULT_STACK_SIZE);  // Declare External Collision as a thread/process
osTimerDef(Wdtimer, Watchdog); // Declare a watch dog timer


// Global variables for interrupt handler
// Global variables for interrupt handler
int32_t dPos_R, dTime_R, Vel_R;
int32_t dPos_L, dTime_L, Vel_L;
int32_t Setspeed_R=0;
int32_t Setspeed_L=0;
int32_t u_R=0,uS_R=0;
int32_t u_L=0,uS_L=0;
int32_t uP_R,uI_R;
int32_t uP_L,uI_L;
int32_t e_R,e_L;
int32_t x_R=0, xT_R=0;
int32_t x_L=0, xT_L=0;

float T = 5000;  //us
float y_R=0;
float y_L=0;
float Kp_R=2.5, Ki_R=0.01;  
float Kp_L=2.5, Ki_L=0.01;  



//**************************************************************************
//**************************************************************************


int main()
{


    Initialize();

    //Userinterface();
    
    //UsRangefinder();
    
    //RcServoMotor();
    
 

    PeriodicInt.attach(&PiControllerISR, 0.0005);
    
    
        do {
        
        //BluetoothModule();

        Userinterface();

        pc.printf("\r\nSR:%d       Vel_R:%d       ER:%d        XR:%d       uR:%d \r\n" ,Setspeed_R,Vel_R,e_R,x_R,u_R);
        
        pc.printf("\r\nSL:%d       Vel_L:%d       EL:%d        XL:%d       uL:%d \r\n\n\n" ,Setspeed_R,Vel_R,e_R,x_R,u_R);
       
       
        wait(0.5); // Go to sleep for 500 ms

    } while(true);
    
    

}




//------------------------------------------------------------
void BluetoothModule(void) {
     // Echo characters at serial Bluetooth Channel (p9, p10)to and from the pc.serial.port
     // Connect TxD from Bluetooth to rx (p10); RxD from Bluetooth to tx (p9)
     char x;
     BluetoothSerial.baud(9600);
     printf("\r\nBluetooth serial channel.\r\n");
     x=0;
     
     if (pc.readable()){
         x=pc.getc();
         BluetoothSerial.putc(x); // Receive keyboard entry and send to bluetooth
         pc.putc(x); //Echo character from MBED COMM
     }
     if (BluetoothSerial.readable()){
         x=BluetoothSerial.getc(); // Receive character from bluetooth module
         pc.putc(x); // Write character from Bluetooth module to terminal emulator on PC
         BluetoothSerial.putc(x); // Echo character character from bluetooth COMM
     }

 }
 

void UsRangefinder(void)
{
    unsigned int V, duration;
    float distance;
    DE0.format(16,1); // SPI is mode 1
    DE0.frequency(500000); // SPI frequency

    IoReset=0; // Reset all SPI peripherals
    IoReset=1;
    wait_us(5);
    IoReset=0;

    SpiReset=0;
    SpiReset=1;
    wait_us(5);
    SpiReset=0;
    V=DE0.write(0x8901); // Control word to read ultrasonic range finder.
    do {
        duration = DE0.write(0x0000); // Read ultrasonic rangefinder.
        pc.printf("\r\n SPI ID=%4x Us Range=%5d",V,duration);
        
        //Need to check if duration is singed or unsigned value, if extend 16bit to 32bit
        distance = (duration / 2) * 0.0344;
        
        if (distance >= 400 || distance <= 2){
            pc.printf("\r\nOut of Range\r\n");
            }
        else {
            pc.printf("Distance = %2f", distance);
            wait(1);
            }
        wait(0.5);
    } while(!pc.readable());
}

void RcServoMotor(void)
{
    unsigned int V, W, X, n;
    DE0.format(16,1); // SPI is mode 1
    DE0.frequency(500000); // SPI frequency
    IoReset=0; // Reset all SPI peripherals
    IoReset=1;
    wait_us(5);
    IoReset=0;

    SpiReset=0;
    SpiReset=1;
    wait_us(5);
    SpiReset=0;
    V=DE0.write(0x0402); // Control word to write RC Servo pulse interval in ms.
    //n=4000;
    
    do {
        /*
        wait_us(10);
        W=DE0.write(0x10); // Write RC Servo pulse interval in ms
        X=DE0.write(n); // Write position to RC Servo M1.
        n=n+2000;
        if(n==60000) n=4000;
        pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
        wait(1);
    
        */
    
        for(n=0;n<=60000;n=n+6000){
            wait_us(10);
            W=DE0.write(0x10); // Write RC Servo pulse interval in ms
            X=DE0.write(n); // Write position to RC Servo M1.
            pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
            wait(1);
        }
    
        for(n=60000;n>=6000;n=n-6000){
            wait_us(10);
            W=DE0.write(0x10); // Write RC Servo pulse interval in ms
            X=DE0.write(n); // Write position to RC Servo M1.
            pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
            wait(1);
    } 
    
    } while(!pc.readable());
    
}



//**************************************************************************
void Userinterface(void){

    
    char dirkey;
    
    pc.printf("\n\rEnter -W- -A- -S- -D- for Manual Control:\n ");
    pc.scanf("%c",&dirkey);
    pc.printf("\n\rKey Entered:%c\n\n\r",dirkey);
    
    
    //Forward
    if(dirkey=='w'||dirkey=='W') {

        if(Setspeed_R<35) {
            Setspeed_R=Setspeed_R+5;
        } else Setspeed_R=35;

        if(Setspeed_L>-35) {
            Setspeed_L=Setspeed_L-5;
        } else Setspeed_L=-35;
    }
    
    

    //Backward
    else if (dirkey=='s'||dirkey=='S') {

        if(Setspeed_R>-35) {
            Setspeed_R=Setspeed_R-5;
        } else Setspeed_R=-35;

        if(Setspeed_L<35) {
            Setspeed_L=Setspeed_L+5;
        } else Setspeed_L=35;
    }
    
    //Left Turn
    else if (dirkey=='a'||dirkey=='A') {

        if(Setspeed_R<35) {
            Setspeed_R=Setspeed_R+2;
        } else Setspeed_R=35;

        if(Setspeed_L>-35) {
            Setspeed_L=Setspeed_L-1;
        } else Setspeed_L=-35;
        
    }
    
    //Right Turn
    else if (dirkey=='d'||dirkey=='D') {
        
        if(Setspeed_R<35) {
            Setspeed_R=Setspeed_R+1;
        } else Setspeed_R=35;

        if(Setspeed_L>-35) {
            Setspeed_L=Setspeed_L-2;
        } else Setspeed_L=-35;    

    }
    
    //Stop
    else if (dirkey=='x'||dirkey=='X') {
     
        Setspeed_R=0;
        y_R=0;
        
        Setspeed_L=0;
        y_L=0;
        
    }
    
    //Wrong Key Entered
    else pc.printf("\n\rInvalid Key!\n\r "); 

    

}    




//**************************************************************************
void Initialize(void)
{
    //Baud Rate
    //pc.baud (460800); 

    //Intialization Motor
    PwmR.period_us(T);
    PwmL.period_us(T);


    //Initialization QEI - to be exected once(normally)
    DE0.format(16,1); //SPI format:16-bit words, mode 1 protocol or mode 0????????????
    //DE0.frequency(500000);
    IoReset=0;
    IoReset=1;
    IoReset=0;

    SpiReset=0;
    SpiReset=1;
    wait_us(10);
    SpiReset=0;
    int ID = DE0.write(0x8004);  //SPI slave control word to read (only) 2-word transactions starting at base address 0 within the peripheral

    if(ID == 23) printf("\r\n\nSPI Interface is Successful !  ID = %d\r\n\n",ID); //ID=0x0017
    else printf("\r\n********SPI Interface failed!********\r\n\n");


}



//**************************************************************************

void PIController(void){

    dPos_R = DE0.write(0x00);
    dTime_R = DE0.write(0x00);
    
    dPos_L = DE0.write(0x00);
    dTime_L = DE0.write(0x00);

    dPos_R = Signextend(dPos_R);
    dPos_L = Signextend(dPos_L);
   

    Vel_R = (123*dPos_R)/dTime_R;  //radians per second
    Vel_L = (123*dPos_L)/dTime_L;  //radians per second
    

    //Error
    e_R = SatSub(Setspeed_R,Vel_R);
    e_R = SatValue(e_R,70);
    
    e_L = SatSub(Setspeed_L,Vel_L);
    e_L = SatValue(e_L,70);
    


    //xtate
    xT_R = SatAdd(x_R,e_R);
    //xT_R = SatValue(x_R,320000);

    xT_L = SatAdd(x_L,e_L);
    //xT_L = SatValue(x_L,320000);

    //proportional and integral control terms
    uP_R=(Kp_R*e_R);
    uI_R=(Ki_R*x_R);
    
    uP_L=(Kp_L*e_L);
    uI_L=(Ki_L*x_L);
    
    uS_R = SatAdd(uP_R,uI_R);
    u_R = SatValue(uS_R,35);
     
    uS_L = SatAdd(uP_L,uI_L);
    u_L = SatValue(uS_L,35);

    if (u_R==uS_R) x_R = xT_R;  //Integrator windup prevention
    if (u_L==uS_L) x_L = xT_L;  //Integrator windup prevention


    if(u_R >= 0) DirR = 1; else DirR = 0;
    if(u_L >= 0) DirL = 0; else DirL = 1;
    
    y_R=u_R/35.0*T;
    PwmR.pulsewidth_us(abs(y_R));
    
    y_L=u_L/35.0*T;
    PwmL.pulsewidth_us(abs(y_L));
    

}


//**************************************************************************


int SatAdd(int x, int y)
{
    // Detect overflow and saturate result
    int z;
    z = x + y;
    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
    else if ((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
    return z;
}


int SatSub(int x, int y)
{
    int z;
    z = x - y;
    // 32-bit overflow detection and saturating arithmetic
    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
    return z;
}


int SatValue(int x, int Limit)
{
    // Impose maximum limit on integrator state
    if(x > Limit) return(Limit);
    else if(x < -Limit) return(-Limit);
    else return(x);
}


int Signextend(int x) {
    if (x & 0x00008000) {
        x = x | 0xFFFF0000;
    }
    return x;
}


//**************************************************************************
//**************************************************************************


// ******** Control Thread ********
void PiControlThread(void const *argument)
{
    while (true) {
        osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received.
        led2= !led2; // Alive status - led2 toggles each time PiControlThread is signaled.

        PIController();

    }
}

// ******** Collision Thread ********
void ExtCollisionThread(void const *argument)
{
    while (true) {
        osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received
        led4 = 1;
    }
}

// ******** Watchdog Interrupt Handler ********
void Watchdog(void const *n)
{
    led3=1; // led3 is activated when the watchdog timer times out
}

// ******** Period Timer Interrupt Handler ********
void PiControllerISR(void)
{
    osSignalSet(PiControl,0x1); // Activate the signal, PiControl, with each periodic timer interrupt.

}

// ******** Collision Interrupt Handler ********
void ExtCollisionISR(void)
{
    osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt.
}

//**************************************************************************