File content as of revision 6:96d4e0fa0827:

// EE4333 Robotics Lab 3

// Library Imports

#include "mbed.h"
#include "Serial.h"
#include "stdio.h"

// Function Declarations

    // DE0
    void DE0_Init(int);     // Done, AS - 1/4/16
    
    // Motor
    void MotorInit(void);   // Done, AS - 1/4/16

    // Control Thread
    void ControlThread(void);
    void IntegratorInit(void);
    
    // Computational Functions
    float SaturateLimit(float x, float limit);  // Done, AS - 1/4/16
    signed int SignExtend(signed int x);

// ********************************************************************
//                     Global Variable Declarations
// ********************************************************************

    float setpoint_L;                  // Desired speed (mm/sec)
    float setpoint_R;
    
    float error_L;                          // Velocity Error (mm/sec)
    float error_R;   
           
    float amp_L;                            // PWM Amplifier Signal
    float amp_R;
    
    float int_L;                            // Integrator States
    float int_R;
    
    signed int dPositionLeft;               // Position Data retrieved from DE0
    signed int dPositionRight;
    
    int dTimeLeft;                          // Time Data retrieved from DE0
    int dTimeRight;
    
    float omega_L;                          // Angular Velocity of Wheels
    float omega_R;
    
    signed int PositionLeft;                // Incremented Position
    signed int PositionRight;
    
    float Rw = 2*2.54;                      // Radius of Wheels ( cm )
    float L = 5.5*2.54;                     //Distance between wheel contacts ( cm )
    float pi = 3.14159265359;
    

// *********************************************************************
//                          Pin Declarations - Done, AS - 1/4/16
// *********************************************************************
    
    DigitalOut DirL(p29);                   // Direction of Left Motor
    DigitalOut DirR(p30);                   // Direction of Right Motor

    // SPI Related Digital I/O Pins
    
    DigitalOut SpiReset(p11);
    DigitalOut IoReset(p12);

    // PWM

    PwmOut PwmL(p22);
    PwmOut PwmR(p21);

    // Communication Channels

    Serial pc(USBTX, USBRX);                // TX & RX for serial channel via USB cable
    Serial Bluetooth(p9,p10);               // TX(p9) , RX(p10) for bluetooth serial channel

    // SPI : Communication wih DE0

    SPI DE0(p5,p6,p7);                      //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
  
    //Interrupts
  
    Ticker ControlInterrupt;                // Internal Interrupt to trigger Control Thread


// **************************************************************************************************
//                      DE0 Init - Done, AS - 1/4/16
// **************************************************************************************************

void DE0_Init(int SpiControlWord){
    
    int mode = 1;
    int bits = 16;
   
    DE0.format(bits,mode);
    
    // Verify Peripheral ID
    
        // Generates single square pulse to reset DE0 IO
        
        IoReset = 0;
        IoReset = 1;
        IoReset = 0;
        
        // Generates single square pulse to reset DE0 SPI
        
        SpiReset = 0;
        SpiReset = 1;
        SpiReset = 0;
        
        // Writes to DE0 Control Register
        
        int ID = DE0.write(SpiControlWord);             // SPI Control Word specifies SPI settings
        
        if(ID == 23){                                   // DE0 ID 23 (0x0017)
            pc.printf("\n\r >> DE0 Initialized.\n\r");}
        else{
            pc.printf("\n\r >> Failed to initialize DE0 board.\n\r");}
    }
    
// **************************************************************************************************
//              Motor Initialization - Done, AS - 1/4/16
// **************************************************************************************************
void MotorInit(void){
    
    int T = 100;
    DirL = 0;
    DirR = 1;
    
    PwmL.period_us(T);
    PwmL.write(0); // Motor Initially Off
    
    PwmR.period_us(T);
    PwmR.write(0); // Motor Initially Off
}

// **************************************************************************************************
//                 Control Thread - Done, AS - 1/4/16
// **************************************************************************************************

void ControlThread(void){

    // Read Incremental Position from DE0 QEI
    
    int dummy = 0x0000;
    
    dPositionLeft = SignExtend(DE0.write(dummy));
    dTimeLeft = DE0.write(dummy);
    
    dPositionRight = SignExtend(DE0.write(dummy));
    dTimeRight = DE0.write(dummy);
    
    // Update Total Incremented Position
    
    PositionLeft = PositionLeft + dPositionLeft;
    PositionRight = PositionRight + dPositionRight;
    
    // Computer Angular Speed
    
    omega_L = (49*dPositionLeft)/dTimeLeft;
    omega_R = (49*dPositionRight)/dTimeRight;
    
    error_L = setpoint_L - omega_L;
    error_R = setpoint_R - omega_R;
    
    // PI Controller Gains
    
    float Kp_L = 4;
    float Ki_L = 0.010;
    
    float Kp_R = 4;
    float Ki_R = 0.010;
       
    // Saturate Inegrators if necessary, currently not checking for overflow in the addition
    
    if(abs(SaturateLimit((Kp_L*error_L+Ki_L*int_L)/14,1))<1){
        int_L = int_L + error_L;}
    else{
        int_L = int_L;
    }
    
    if(abs(SaturateLimit((Kp_R*error_R+Ki_R*int_R)/14,1))<1){    //Checks that we are not saturating the Left integrator before summing more error
        int_R = int_R + error_R;}
    else{
        int_R = int_R;
    }
    
    amp_L = SaturateLimit((Kp_L*error_L + Ki_L*int_L),1);
    amp_R = SaturateLimit((Kp_R*error_R + Ki_R*int_R),1);
    
    //Determine direction bits based on sign of required output signal
    
    if(amp_L <=0)
        DirL = 0;
    else
        DirL = 1;
    
    if(amp_R <=0)
        DirR = 1;
    else
        DirR = 0;
    
    //Write final values to the PWM 
       
    PwmL.write(abs(amp_L));
    PwmR.write(abs(amp_R));
}

/*// *************************************************
//                SaturateAdd - Not Used
// ***************************************************

signed int SaturateAdd(signed int x, signed int y){
    
    signed int z = x + y;
    
    if( (x>0) && (y>0)&& (z<=0) ){
        z = 0x7FFFFFFF;}
    
    else if( (x<0)&&(y<0)&&(z>=0) ){
        z = 0x80000000;}
        
    return z;
}*/
    
// ***************************************************
//              SaturateLimit - Done, AS - 1/4/16
// ***************************************************

float SaturateLimit(float x, float limit){              //Ensures we don't ask for more than the PWM can give.
    
    if (x > limit){
        return limit;
        }
    else if(x < -limit){
        return(-limit);
        }
    else{
        return x;}
        
}

/// ***************************************************
//                   SignExtend - Done, AS - 1/4/16
// ***************************************************

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

/// ***************************************************
//                   SignExtend - Done, AS - 1/4/16
// ***************************************************

void IntegratorInit(void){
        
        int_L = 0;
        int_R = 0;

}
// *********************************************************************
//                        MAIN FUNCTION
// *********************************************************************

int main(){
    
   // Initialization 

    DE0_Init(0x8004);                                                
    MotorInit();
    ControlInterrupt.attach(&ControlThread, 0.00025);
    IntegratorInit();
    float dL,dR,v,vR,vL;
    
    //float Motion[5][2] = { {30.5,47.9} , {10000,152.4} , {-61,287.3} , {10000,91.4} , {45.7,143.6} }; % Using 10k to represent inf
    
    float R[5]={30.5,100000,-61,100000,45.7};
    float d[5]={47.9*1.05,152.4,287.3,91.4,143.6};
    
    pc.printf("\n\r ready to work");
    wait(5);
    //int i=0;
    for( int i = 0 ; i < 5 ; i++ ){
        
        //pc.printf("\n\r off i go then");
        dL = d[i]*(1+L/(2*R[i]))/Rw*(851400/6804)*(100/pi)/1.9;
        dR = d[i]*(1-L/(2*R[i]))/Rw*(851400/6804)*(100/pi)/1.9;
        
        v = 25;
        setpoint_R = v*(1-L/(2*R[i]))/Rw;
        setpoint_L = v*(1+L/(2*R[i]))/Rw;
        
        pc.printf("\n\r%2.2f",d[i]);
        pc.printf("\n\r%2.2f",R[i]);
        pc.printf("\n\r %2.2f",dL);
        pc.printf("\n\r %2.2f",dR);
        printf("\n\r%2.2f",setpoint_R);
        printf("\n\r%2.2f",setpoint_L);
        
        while((PositionLeft<dL) || (PositionRight<dR)){
            pc.printf("\n\r%2.2f, %2.2f",setpoint_L,omega_L);
            pc.printf("\n\r%2.2f, %2.2f",setpoint_R,omega_R);
            pc.printf("\n\r");
            wait(0.1);
        }
        
        PositionLeft = 0;
        PositionRight = 0;
    }
    setpoint_R = 0;
    setpoint_L = 0;
}