Diff: MotionPrimitive.cpp

Revision:

6:96d4e0fa0827

Parent:

5:ac5819613d0c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionPrimitive.cpp	Tue Apr 05 19:32:11 2016 +0000
@@ -0,0 +1,320 @@
+// 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;
+}