File content as of revision 0:867c4eabf128:

#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 BluetoothTest(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 QeiInterface(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
int32_t dPos_R, dTime_R, Vel_R;
int32_t Setspeed_R;
int32_t u_R=0,uS_R=0;
int32_t uP_R,uI_R;
int32_t e_R;
int32_t x_R=0, xT_R=0;

float T = 5000;  //us
float y_R=0;
float Kp_R=3.5, Ki_R=0.8;  


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


int main()
{

    //Initialize();

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

}


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)
{

    pc.printf("\n\rEnter number:\n ");
    pc.scanf("%d",&Setspeed_R);
    pc.printf("\n\rYou Entered :%d\n\n",Setspeed_R);

    /*
    DirR=1;
    y_R=Setspeed_R/36*T;
    pc.printf("y_R = %f\n",y_R);
    PwmR.pulsewidth_us(y_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(0x8002);  //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 QeiInterface(void)
{

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

    dPos_R = Signextend(dPos_R);
   

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

    printf("\r\ndPos_R: %d dTime_R: %d Vel_R: %d \r\n",dPos_R,dTime_R,Vel_R);


}

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

void PIController(void){

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

    dPos_R = Signextend(dPos_R);
    

    Vel_R = (123*dPos_R)/dTime_R; 
    

    //Error
    e_R = SatSub(Setspeed_R,Vel_R);
    //e_R = SatValue(e_R,36);


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


    //proportional and integral control terms
    uP_R=(Kp_R*e_R);
    uI_R=(Ki_R*x_R);
    
    uS_R = SatAdd(uP_R,uI_R);
    u_R = SatValue(uS_R,36);

    if (u_R==uS_R) x_R = xT_R;  //Integrator windup prevention

    if(u_R >= 0) DirR = 1; else DirR = 0;
    
    y_R=u_R/36.0*T;
    PwmR.pulsewidth_us(abs(y_R));
    

}


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

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

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