ECE 4333
motion primitives
Dependencies: mbed
Fork of
Motion_Primitive_Test
by 
ECE 4333
Revision 6:96d4e0fa0827, committed 2016-04-05
- Comitter:
- A_Sterner
- Date:
- Tue Apr 05 19:32:11 2016 +0000
- Parent:
- 5:ac5819613d0c
- Commit message:
- Motion primitives;
Changed in this revision
| MotionPrimitive.cpp | Show annotated file Show diff for this revision Revisions of this file |
| MotionTest.cpp | Show diff for this revision Revisions of this file |
diff -r ac5819613d0c -r 96d4e0fa0827 MotionPrimitive.cpp
--- /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;
+}
diff -r ac5819613d0c -r 96d4e0fa0827 MotionTest.cpp
--- a/MotionTest.cpp Sat Mar 19 19:50:53 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,394 +0,0 @@
-// 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;
- float AngularSpeedLeft;
- float AngularSpeedRight;
- //signed int AngularSpeedLeft;
- //signed int AngularSpeedRight;
-
- //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)); //Extends the 16-bit signed ints into 32-bit containers
- dTimeLeft = DE0.write(dummy);
- dPositionRight = SignExtend(DE0.write(dummy));
- dTimeRight = DE0.write(dummy);
-
- // Computer Angular Speed and Angular Speed Error
-
- AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft; //123 is the constant derived from the gear ratios and properties of the motor
- 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;
-
- // Saturate Inegrators if necessary
- if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){ //Checks that we are not saturating the Left integrator before summing more error
- 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){ //Checks that we are not saturating the Left integrator before summing more error
- R_integrator = R_integrator +R_e;}
- else{
- R_integrator = R_integrator;
- }
-
- L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1); //Checks that we are not saturating the Left integrator before summing more error
- R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1); //Checks that we are not saturating the Left integrator before summing more error
-
- //Determine direction bits based on sign of required output signal
- if(L_u <=0)
- DirL = 0;
- else
- DirL = 1;
-
- if(R_u <=0)
- DirR = 1;
- else
- DirR = 0;
-
- //Write final values to the PWM
- PwmL.write(abs(L_u));
- PwmR.write(abs(R_u));
-}
-
-/*// *************************************************** NOT USED
-// 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){ //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;}
-
-}
-
-/// ***************************************************
-// Sign Extend
-// ***************************************************
-
-signed int SignExtend(int x){
-
- if(x&0x00008000){
- x = x|0xFFFF0000;
- }
-
- return x;
-}
-
-// ***************************************************
-// Startup GUI
-// ***************************************************
-
-char GUI(){
- char x;
- Bluetooth.printf("\n\r (C)ircle or (L)ine? >>"); //Determine Trajectory Type
- Bluetooth.scanf("%c", &x);
- return x; //Return trajectory type
- }
-
-// ***************************************************
-// Setpoint Collection
-// ***************************************************
-
-
-void Settings(char x){
- char dir; //Direction Variable
- float dist; //Distance Variable
- if(x == 'c' || x == 'C') //If the user chooses a circle
- {
- Bluetooth.printf("\n\rCircle Radius? (m)"); //Determine circle radius
- Bluetooth.scanf("%f", &Radius);
- Bluetooth.printf("\n\rDesired Speed? (m)"); //Speed
- Bluetooth.scanf("%f", &VelDes);
- Bluetooth.printf("\n\r Desired Direction? (L) or (R)"); //Direction of Travel
- Bluetooth.scanf("%c",&dir);
- Bluetooth.printf("\n\r Desired Distance? (m)"); //Determine maximum distance
- Bluetooth.scanf("%c",&dist);
-
- float Circ = 2*pi*Radius;
- float tTot = Circ/VelDes;
-
- if(dir == 'l' || dir == 'L') //If counterclockwise direction, the right wheel is faster
- {
- I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot); //Set right wheel speed
- if(I_setpoint>35)
- {
- I_setpoint = 35;
- Bluetooth.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);
- return
- }
- }
- else //Clockwise, the left wheel is faster
- {
- O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot); //Set Left wheel speed
- if(O_setpoint>35)
- {
- O_setpoint = 35;
- Bluetooth.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 //If the user chooses a line instead
- {
- Bluetooth.printf("\n\r Desired Speed? (m) >>"); //We only need maximum speed and maximum distance
- Bluetooth.scanf("%f", &VelDes);
- Bluetooth.printf("\n\r Desired Distance? (m)"); //Determine maximum distance
- Bluetooth.scanf("%c",&dist);
- O_setpoint = VelDes/0.05094; //Saturate if necessary
-
- if(O_setpoint>35)
- {
- O_setpoint = 35;
- Bluetooth.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
- }
- I_setpoint = O_setpoint; //For a straight line both wheel speeds must be equal
- }
-
-
- return;
-}
-
-
-// *********************************************************************
-// MAIN FUNCTION
-// *********************************************************************
-
-int main(){
-
- // Initialization
-
- DE0_Init(0x8004);
- L_MotorInit();
- R_MotorInit();
- L_integrator = 0;
- R_integrator = 0;
- ControlInterrupt.attach(&ControlThread, 0.0005);
- float WAvg,VAvg; //Reported Speeds
-
- //Motion Primitives Initialization
- Arc = 0; // Arc Length (start at 0)
- Settings(GUI()); // Ask user for input and pass to Settings
-
-
- // Display Global Variables to Console
- while(1){
- WAvg = (AngularSpeedLeft + AngularSpeedRight); //Angular Average
- VAvg = (AngularSpeedLeft + AngularSpeedRight)*0.02483*2; //Velocity Average with unit conversion
- Bluetooth.printf("\n\r||SetLeft: %2.3f ||WLeft: %2.3f rad/s ||SetRight: %2.3f ||WRight: %2.3f rad/s||WAverage: %2.3f||VAverage: %2.3f",O_setpoint,AngularSpeedLeft,I_setpoint,AngularSpeedRight,WAvg,VAvg);
- wait(0.75);
- }
-}