Diff: MotionTest.cpp

Revision:

0:4c63f2192998

Child:

1:d523415c7b53

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionTest.cpp	Fri Mar 11 17:10:59 2016 +0000
@@ -0,0 +1,422 @@
+// 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);
+
+// ********************************************************************
+//                     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;
+    
+    //Motion Primitive Variables
+    signed int Arc;
+    float Radius;
+    float VelDes;
+    float tcalc;
+    int tcum;
+    float O_setpoint;                     //Inner Wheel Speed
+    float I_setpoint;                     //Outer Wheel Speed
+    float L; //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);
+    
+}
+    
+/// ***************************************************
+//                 User Input Left
+// ***************************************************
+
+signed int InputLeft(void){
+   
+   signed int input;
+   
+   Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (rads/sec) >> ");
+   Bluetooth.scanf("%i",&input);
+   
+   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
+   
+   return input;
+
+}
+
+signed int InputLeft_m(void){
+   
+   signed int input;
+   
+   Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (m/sec) >> ");
+   Bluetooth.scanf("%i",&input);
+   
+   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
+   
+   return input;
+
+}
+
+/// ***************************************************
+//                 User Input Right
+// ***************************************************
+
+signed int InputRight(void){
+      
+   signed int input;
+   
+   Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (rads/sec) >> ");
+   Bluetooth.scanf("%i",&input);
+   
+   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
+   
+   return input;
+
+}
+
+signed int InputRight_m(void){
+      
+   signed int input;
+   
+   Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (m/sec) >> ");
+   Bluetooth.scanf("%i",&input);
+   
+   Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
+   
+   return input;
+
+}
+
+/// ***************************************************
+//                 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;
+}
+
+
+
+// ==============================================================================================================
+// ==============================================================================================================
+
+
+// *********************************************************************
+//                        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) 
+   L = 0.14605; //Distance between wheel contacts  
+   
+   //IF RADIUS IS TOO LOW OR VELOCITY TOO HIGH THINGS GO CRAZY 
+   Radius = 0.2; // Minimum L/2;
+   VelDes = 1.7;
+   tcum = 0;
+   float circum = 2*pi*Radius; //Arc circumference
+   
+   tcalc = circum/VelDes;      // The easiest way to get the right speeds is to determine the theoretical completion time
+   // Inner and Outer Wheel Setpioints
+   O_setpoint = ((Radius+(L/2))*2*pi)/(2*2.54*tcalc/100); //0.02*2.54*tcalc is for rads/s
+   I_setpoint = ((Radius-(L/2))*2*pi)/(2*2.54*tcalc/100); 
+   pc.printf("Out: %2.2f   In: %2.2f \n\r", O_setpoint,I_setpoint);
+   
+   // Specify Setpoint ( rads/sec )
+    
+   //L_setpoint = InputLeft();
+   //R_setpoint = InputRight();
+   
+   // Specify Setpoint ( m/sec )
+   /* 
+   L_setpoint = InputLeft_m();
+   R_setpoint = InputRight_m();*/
+        
+   // Display Global Variables to Console
+   while(1){
+       
+       float L_error_t = L_e;
+       float L_error_m = L_e*0.05093;                       //Multiply by 0.05093 for m/s
+       float L_u_t = L_u;     
+       
+       float R_error_t = R_e;                       //Multiply by 0.05093 for m/s
+       float R_u_t = R_u;
+       float R_error_m = R_e*0.05093;                       //Multiply by 0.05093 for m/s
+            
+       float L_setpoint_m = L_setpoint*0.05093;               //Setpoints in float values for display
+       float R_setpoint_m = R_setpoint*0.05093;               //Setpoints in float values for display
+
+       pc.printf("Radius: %2.2f m Velocity: %2.2f rad/s\n\r", Radius,VelDes);
+       if (O_setpoint > 35)
+       {
+       pc.printf("L_Set:   MAX  R_Set:   MAX \n\r");
+       O_setpoint = 35;
+       I_setpoint = O_setpoint*(Radius-L/2)/(Radius+L/2);
+       }
+       else
+       {    
+       pc.printf("L_Set:   %2.2f  R_Set:   %2.2f \n\r",O_setpoint,I_setpoint);           
+       }
+       wait(0.75);
+   }
+   
+}