File content as of revision 2:0f93881225d2:

// EE4333 Robotics Lab 3

// Library Imports

//#include "InterruptIn.h"
//#include "rtos.h"
#include "mbed.h"
#include "Serial.h"
#include "stdio.h"

//Function Declarations

    void DE0_Init(int);
    void L_MotorInit(void);
    void R_MotorInit(void);
    signed int InputLeft(void);
    signed int InputRight(void);
    void ControlThread(void);
    int SaturateAdd(int x, int y);
    float SaturateLimit(float x, float limit);
    signed int SignExtend(signed int x);
    char GUI();
    void Settings(char x);

// ********************************************************************
//                     GLOBAL VARIABLE DECLARATIONS
// ********************************************************************

    signed int R_setpoint;                // Desired Angular Speed ( rad/sec )
    signed int L_setpoint;
    
    signed int R_setpoint_m;              // Desired Angular Speed ( m/sec )
    signed int L_setpoint_m;
    
    float R_e;                            // Velocity Error
    float R_u;                            // Control Signal
    float L_e;
    float L_u;
    int L_integrator;                   // Left Integrator State
    int R_integrator;
    signed int dPositionLeft;           // DE0 Register 0
    signed int dPositionRight;
    int dTimeLeft;                      // DE0 Register 1
    int dTimeRight;
    float Circ = 0;
    float tTot = 0;
    
    //Motion Primitive Variables
    signed int Arc;
    float Radius = 0;
    float VelDes = 0;
    float tcalc = 0;

    float O_setpoint;                     //Inner Wheel Speed
    float I_setpoint;                     //Outer Wheel Speed
    float L = 5.5*2.54/100; //Distance between wheel contacts  ; //Distance between wheels (in meters)
    float pi = 3.141259;
// *********************************************************************
//                     PROCESSES AND THREADS
// *********************************************************************


// *********************************************************************
//                          PIN DECLARATIONS
// *********************************************************************

    // Digital I/O Pins

    DigitalOut led1(LED1);              // Thread Indicators
    DigitalOut led2(LED2);              //
    DigitalOut led3(LED3);              //
    DigitalOut led4(LED4);              //
    
    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);

    //Serial

    Serial pc(USBTX, USBRX);            // tx and rx for PC serial channel via USB cable
    Serial Bluetooth(p9,p10);           // Pins tx(p9) , rx(p10) for bluetooth serial channel

    //SPI

    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
// ***************************************************

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)
        Bluetooth.printf("\n\r >> DE0 Initialized.\n\r");}
    else{
        Bluetooth.printf("\n\r >> Failed to initialize DE0 board.\n\r");}
}

// ***************************************************
//              Left Motor Initialization
// ***************************************************

    // Pwm Pin Left Motor : p21
    // Direction Pin Left Motor : p29

void L_MotorInit(void){

   DirL = 1;                                   // Defaults to 1

    // Direction bit logic output
    // 0 : Backwards ( Reverse ) 
    // 1 : Forwards  ( Advance )

    PwmL.period_us(100);
    PwmL.write(0);
    
}
    
// ***************************************************
//             Right Motor Initialization
// ***************************************************

    // Pwm Pin Right Motor : p22
    // Direction Pin Right Motor : p30
    
void R_MotorInit(void){
    
   DirR = 0;                                   // Defaults to 0.
    
    // Direction bit logic output
    // 0 : Forwards  ( Advance ) 
    // 1 : Backwards ( Reverse )
    
    PwmR.period_us(100);
    PwmR.write(0);   
}
    

/// ***************************************************
//                 Control Thread
// ***************************************************

void ControlThread(void){

    // Read Incremental Position from DE0 QEI
    int dummy = 0x0000;                                // Pushes dummy information which DE0 ignores, store return from QEI register
    
    dPositionLeft = SignExtend(DE0.write(dummy));
    dTimeLeft = DE0.write(dummy);
    dPositionRight = SignExtend(DE0.write(dummy));
    dTimeRight = DE0.write(dummy);
    
    // Computer Angular Speed and Angular Speed Error
    
    signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
    signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
    
    //CHANGED FOR OUTER AND INNER WHEELS
    L_e = O_setpoint - AngularSpeedLeft;
    R_e = I_setpoint - AngularSpeedRight;
    
    float Kp_L = 2.5;
    float Ki_L = 0.010;
    
    float Kp_R = 2.5;
    float Ki_R = 0.010;
    
    //          **** MODIFICATIONS FOR MOTION PRIMITIVE ARE HERE ****
    // In order to determine theta relative to any given position, we can just use Theta = Arc Length/Radius
    // Radius R is determined by the user
    // Arc Length is determined from an integration of average velocity dP/dt, which is tracked by the DE0 board
    
    //Arc Length (meters) variable - We have speed and time outputs already so we can use those values to calculate average arc of V
    //Arc = (AngularSpeedLeft*dTimeLeft+AngularSpeedRight*dTimeRight)*0.05093/2; //constant of 0.05093 derived in another function using wheel radii
      
    // Saturate Inegrators if necessary
    if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
        L_integrator = L_integrator +L_e;}
    else{
        L_integrator = L_integrator;
    }
    
    if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){
        R_integrator = R_integrator +R_e;}
    else{
        R_integrator = R_integrator;
    }
 
    L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
    R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
        
    if(L_u <=0)
        DirL = 0;
    else
        DirL = 1;
    
    if(R_u <=0)
        DirR = 1;
    else
        DirR = 0;
        
    PwmL.write(abs(L_u));
    PwmR.write(abs(R_u));
}

/// ***************************************************
//                SaturateAdd
// ***************************************************

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
// ***************************************************

float SaturateLimit(float x, float limit){
    
    if (x > limit){
        return limit;
        }
    else if(x < -limit){
        return(-limit);
        }
    else{
        return x;}
        
}

/// ***************************************************
//                   Sign Extend
// ***************************************************

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

char GUI(){
    char x;
    pc.printf("\n\r (C)ircle or (L)ine? >>");
    pc.scanf("%c", &x);
    return x;
    }

void Settings(char x){
    char z;
    if(x == 'c' || x == 'C')
    {
        pc.printf("\n\rCircle Radius? (m)");
        pc.scanf("%f", &Radius);
        pc.printf("\n\rDesired Speed? (m)");
        pc.scanf("%f", &VelDes);
        pc.printf("\n\r Desired Direction? (L) or (R)");
        pc.scanf("%c",&z);
        float Circ = 2*pi*Radius;
        float tTot = Circ/VelDes;
        if(z == 'l' || z == 'L')
        {
            I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);    
            if(I_setpoint>35)
            {
                I_setpoint = 35;
                pc.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
            }
            O_setpoint = I_setpoint*(Radius-L/2)/(Radius+L/2);
        }
        else
        {
            O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
            if(O_setpoint>35)
            {
                O_setpoint = 35;
                pc.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
                
            }
            I_setpoint = (Radius-L/2)*2*pi/((2*2.54/100)*tTot);    
        }
    }
    else
    {
        pc.printf("\n\r Desired Speed? (m) >>");
        pc.scanf("%f", &VelDes);
        O_setpoint = VelDes/0.05094;
        if(O_setpoint>35)
        {
            O_setpoint = 35;
            pc.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
        }
        I_setpoint = O_setpoint;
    }    
    return;
    }

// ==============================================================================================================
// ==============================================================================================================


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

int main(){
   
   // Initialization 

   DE0_Init(0x8004);                                                
   L_MotorInit();
   R_MotorInit();
   L_integrator = 0;
   R_integrator = 0;
   ControlInterrupt.attach(&ControlThread, 0.0005);
   
   //Motion Primitives Initialization
   Arc = 0;    // Arc Length (start at 0) 
   Settings(GUI()); 
   float SpeedActual; 
   unsigned int VLeft,VRight;
   
   // Display Global Variables to Console
   while(1){ 
       VLeft = 123*(dPositionLeft/dTimeLeft);
       VRight = 123*(dPositionRight/dTimeRight);
       SpeedActual = (VLeft+VRight)/2;
       pc.printf("\n\r||VLeft: %2.2f ||VRight: %2.2f ||Radius: %2.2f",VLeft,VRight,SpeedActual);
       wait(1);
   } 
}