ECE 4333 / Mbed 2 deprecated Motion_Primitive

motion primitives

Dependencies:   mbed

Fork of Motion_Primitive_Test by ECE 4333

MotionPrimitive.cpp@6:96d4e0fa0827, 2016-04-05 (annotated)

Committer:
A_Sterner
Date:
Tue Apr 05 19:32:11 2016 +0000
Revision:
6:96d4e0fa0827
Parent:
MotionTest.cpp@5:ac5819613d0c 
Motion primitives;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JordanWisdom 0:4c63f2192998 1 // EE4333 Robotics Lab 3
JordanWisdom 0:4c63f2192998 2
JordanWisdom 0:4c63f2192998 3 // Library Imports
JordanWisdom 0:4c63f2192998 4
JordanWisdom 0:4c63f2192998 5 #include "mbed.h"
JordanWisdom 0:4c63f2192998 6 #include "Serial.h"
JordanWisdom 0:4c63f2192998 7 #include "stdio.h"
JordanWisdom 0:4c63f2192998 8
A_Sterner 6:96d4e0fa0827 9 // Function Declarations
A_Sterner 6:96d4e0fa0827 10
A_Sterner 6:96d4e0fa0827 11 // DE0
A_Sterner 6:96d4e0fa0827 12 void DE0_Init(int); // Done, AS - 1/4/16
A_Sterner 6:96d4e0fa0827 13
A_Sterner 6:96d4e0fa0827 14 // Motor
A_Sterner 6:96d4e0fa0827 15 void MotorInit(void); // Done, AS - 1/4/16
JordanWisdom 0:4c63f2192998 16
A_Sterner 6:96d4e0fa0827 17 // Control Thread
JordanWisdom 0:4c63f2192998 18 void ControlThread(void);
A_Sterner 6:96d4e0fa0827 19 void IntegratorInit(void);
A_Sterner 6:96d4e0fa0827 20
A_Sterner 6:96d4e0fa0827 21 // Computational Functions
A_Sterner 6:96d4e0fa0827 22 float SaturateLimit(float x, float limit); // Done, AS - 1/4/16
JordanWisdom 0:4c63f2192998 23 signed int SignExtend(signed int x);
JordanWisdom 0:4c63f2192998 24
JordanWisdom 0:4c63f2192998 25 // ********************************************************************
A_Sterner 6:96d4e0fa0827 26 // Global Variable Declarations
JordanWisdom 0:4c63f2192998 27 // ********************************************************************
JordanWisdom 0:4c63f2192998 28
A_Sterner 6:96d4e0fa0827 29 float setpoint_L; // Desired speed (mm/sec)
A_Sterner 6:96d4e0fa0827 30 float setpoint_R;
JordanWisdom 0:4c63f2192998 31
A_Sterner 6:96d4e0fa0827 32 float error_L; // Velocity Error (mm/sec)
A_Sterner 6:96d4e0fa0827 33 float error_R;
A_Sterner 6:96d4e0fa0827 34
A_Sterner 6:96d4e0fa0827 35 float amp_L; // PWM Amplifier Signal
A_Sterner 6:96d4e0fa0827 36 float amp_R;
A_Sterner 6:96d4e0fa0827 37
A_Sterner 6:96d4e0fa0827 38 float int_L; // Integrator States
A_Sterner 6:96d4e0fa0827 39 float int_R;
A_Sterner 6:96d4e0fa0827 40
A_Sterner 6:96d4e0fa0827 41 signed int dPositionLeft; // Position Data retrieved from DE0
JordanWisdom 0:4c63f2192998 42 signed int dPositionRight;
A_Sterner 6:96d4e0fa0827 43
A_Sterner 6:96d4e0fa0827 44 int dTimeLeft; // Time Data retrieved from DE0
JordanWisdom 0:4c63f2192998 45 int dTimeRight;
A_Sterner 6:96d4e0fa0827 46
A_Sterner 6:96d4e0fa0827 47 float omega_L; // Angular Velocity of Wheels
A_Sterner 6:96d4e0fa0827 48 float omega_R;
JordanWisdom 0:4c63f2192998 49
A_Sterner 6:96d4e0fa0827 50 signed int PositionLeft; // Incremented Position
A_Sterner 6:96d4e0fa0827 51 signed int PositionRight;
A_Sterner 6:96d4e0fa0827 52
A_Sterner 6:96d4e0fa0827 53 float Rw = 2*2.54; // Radius of Wheels ( cm )
A_Sterner 6:96d4e0fa0827 54 float L = 5.5*2.54; //Distance between wheel contacts ( cm )
A_Sterner 6:96d4e0fa0827 55 float pi = 3.14159265359;
A_Sterner 6:96d4e0fa0827 56
JordanWisdom 0:4c63f2192998 57
JordanWisdom 0:4c63f2192998 58 // *********************************************************************
A_Sterner 6:96d4e0fa0827 59 // Pin Declarations - Done, AS - 1/4/16
JordanWisdom 0:4c63f2192998 60 // *********************************************************************
JordanWisdom 0:4c63f2192998 61
A_Sterner 6:96d4e0fa0827 62 DigitalOut DirL(p29); // Direction of Left Motor
A_Sterner 6:96d4e0fa0827 63 DigitalOut DirR(p30); // Direction of Right Motor
JordanWisdom 0:4c63f2192998 64
JordanWisdom 0:4c63f2192998 65 // SPI Related Digital I/O Pins
JordanWisdom 0:4c63f2192998 66
JordanWisdom 0:4c63f2192998 67 DigitalOut SpiReset(p11);
JordanWisdom 0:4c63f2192998 68 DigitalOut IoReset(p12);
JordanWisdom 0:4c63f2192998 69
A_Sterner 6:96d4e0fa0827 70 // PWM
JordanWisdom 0:4c63f2192998 71
JordanWisdom 0:4c63f2192998 72 PwmOut PwmL(p22);
JordanWisdom 0:4c63f2192998 73 PwmOut PwmR(p21);
JordanWisdom 0:4c63f2192998 74
A_Sterner 6:96d4e0fa0827 75 // Communication Channels
JordanWisdom 0:4c63f2192998 76
A_Sterner 6:96d4e0fa0827 77 Serial pc(USBTX, USBRX); // TX & RX for serial channel via USB cable
A_Sterner 6:96d4e0fa0827 78 Serial Bluetooth(p9,p10); // TX(p9) , RX(p10) for bluetooth serial channel
JordanWisdom 0:4c63f2192998 79
A_Sterner 6:96d4e0fa0827 80 // SPI : Communication wih DE0
JordanWisdom 0:4c63f2192998 81
A_Sterner 6:96d4e0fa0827 82 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
JordanWisdom 0:4c63f2192998 83
JordanWisdom 0:4c63f2192998 84 //Interrupts
JordanWisdom 0:4c63f2192998 85
A_Sterner 6:96d4e0fa0827 86 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
JordanWisdom 0:4c63f2192998 87
JordanWisdom 0:4c63f2192998 88
A_Sterner 6:96d4e0fa0827 89 // **************************************************************************************************
A_Sterner 6:96d4e0fa0827 90 // DE0 Init - Done, AS - 1/4/16
A_Sterner 6:96d4e0fa0827 91 // **************************************************************************************************
JordanWisdom 0:4c63f2192998 92
JordanWisdom 0:4c63f2192998 93 void DE0_Init(int SpiControlWord){
JordanWisdom 0:4c63f2192998 94
JordanWisdom 0:4c63f2192998 95 int mode = 1;
JordanWisdom 0:4c63f2192998 96 int bits = 16;
JordanWisdom 0:4c63f2192998 97
JordanWisdom 0:4c63f2192998 98 DE0.format(bits,mode);
JordanWisdom 0:4c63f2192998 99
JordanWisdom 0:4c63f2192998 100 // Verify Peripheral ID
JordanWisdom 0:4c63f2192998 101
A_Sterner 6:96d4e0fa0827 102 // Generates single square pulse to reset DE0 IO
A_Sterner 6:96d4e0fa0827 103
A_Sterner 6:96d4e0fa0827 104 IoReset = 0;
A_Sterner 6:96d4e0fa0827 105 IoReset = 1;
A_Sterner 6:96d4e0fa0827 106 IoReset = 0;
A_Sterner 6:96d4e0fa0827 107
A_Sterner 6:96d4e0fa0827 108 // Generates single square pulse to reset DE0 SPI
A_Sterner 6:96d4e0fa0827 109
A_Sterner 6:96d4e0fa0827 110 SpiReset = 0;
A_Sterner 6:96d4e0fa0827 111 SpiReset = 1;
A_Sterner 6:96d4e0fa0827 112 SpiReset = 0;
A_Sterner 6:96d4e0fa0827 113
A_Sterner 6:96d4e0fa0827 114 // Writes to DE0 Control Register
A_Sterner 6:96d4e0fa0827 115
A_Sterner 6:96d4e0fa0827 116 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
A_Sterner 6:96d4e0fa0827 117
A_Sterner 6:96d4e0fa0827 118 if(ID == 23){ // DE0 ID 23 (0x0017)
A_Sterner 6:96d4e0fa0827 119 pc.printf("\n\r >> DE0 Initialized.\n\r");}
A_Sterner 6:96d4e0fa0827 120 else{
A_Sterner 6:96d4e0fa0827 121 pc.printf("\n\r >> Failed to initialize DE0 board.\n\r");}
A_Sterner 6:96d4e0fa0827 122 }
JordanWisdom 0:4c63f2192998 123
A_Sterner 6:96d4e0fa0827 124 // **************************************************************************************************
A_Sterner 6:96d4e0fa0827 125 // Motor Initialization - Done, AS - 1/4/16
A_Sterner 6:96d4e0fa0827 126 // **************************************************************************************************
A_Sterner 6:96d4e0fa0827 127 void MotorInit(void){
JordanWisdom 0:4c63f2192998 128
A_Sterner 6:96d4e0fa0827 129 int T = 100;
A_Sterner 6:96d4e0fa0827 130 DirL = 0;
A_Sterner 6:96d4e0fa0827 131 DirR = 1;
JordanWisdom 0:4c63f2192998 132
A_Sterner 6:96d4e0fa0827 133 PwmL.period_us(T);
A_Sterner 6:96d4e0fa0827 134 PwmL.write(0); // Motor Initially Off
A_Sterner 6:96d4e0fa0827 135
A_Sterner 6:96d4e0fa0827 136 PwmR.period_us(T);
A_Sterner 6:96d4e0fa0827 137 PwmR.write(0); // Motor Initially Off
JordanWisdom 0:4c63f2192998 138 }
JordanWisdom 0:4c63f2192998 139
A_Sterner 6:96d4e0fa0827 140 // **************************************************************************************************
A_Sterner 6:96d4e0fa0827 141 // Control Thread - Done, AS - 1/4/16
A_Sterner 6:96d4e0fa0827 142 // **************************************************************************************************
JordanWisdom 0:4c63f2192998 143
JordanWisdom 0:4c63f2192998 144 void ControlThread(void){
JordanWisdom 0:4c63f2192998 145
JordanWisdom 0:4c63f2192998 146 // Read Incremental Position from DE0 QEI
A_Sterner 6:96d4e0fa0827 147
A_Sterner 6:96d4e0fa0827 148 int dummy = 0x0000;
JordanWisdom 0:4c63f2192998 149
A_Sterner 6:96d4e0fa0827 150 dPositionLeft = SignExtend(DE0.write(dummy));
JordanWisdom 0:4c63f2192998 151 dTimeLeft = DE0.write(dummy);
A_Sterner 6:96d4e0fa0827 152
JordanWisdom 0:4c63f2192998 153 dPositionRight = SignExtend(DE0.write(dummy));
JordanWisdom 0:4c63f2192998 154 dTimeRight = DE0.write(dummy);
JordanWisdom 0:4c63f2192998 155
A_Sterner 6:96d4e0fa0827 156 // Update Total Incremented Position
JordanWisdom 0:4c63f2192998 157
A_Sterner 6:96d4e0fa0827 158 PositionLeft = PositionLeft + dPositionLeft;
A_Sterner 6:96d4e0fa0827 159 PositionRight = PositionRight + dPositionRight;
A_Sterner 6:96d4e0fa0827 160
A_Sterner 6:96d4e0fa0827 161 // Computer Angular Speed
JordanWisdom 0:4c63f2192998 162
A_Sterner 6:96d4e0fa0827 163 omega_L = (49*dPositionLeft)/dTimeLeft;
A_Sterner 6:96d4e0fa0827 164 omega_R = (49*dPositionRight)/dTimeRight;
JordanWisdom 0:4c63f2192998 165
A_Sterner 6:96d4e0fa0827 166 error_L = setpoint_L - omega_L;
A_Sterner 6:96d4e0fa0827 167 error_R = setpoint_R - omega_R;
A_Sterner 6:96d4e0fa0827 168
A_Sterner 6:96d4e0fa0827 169 // PI Controller Gains
A_Sterner 6:96d4e0fa0827 170
A_Sterner 6:96d4e0fa0827 171 float Kp_L = 4;
JordanWisdom 0:4c63f2192998 172 float Ki_L = 0.010;
JordanWisdom 0:4c63f2192998 173
A_Sterner 6:96d4e0fa0827 174 float Kp_R = 4;
JordanWisdom 0:4c63f2192998 175 float Ki_R = 0.010;
JordanWisdom 4:1bcb35cef400 176
A_Sterner 6:96d4e0fa0827 177 // Saturate Inegrators if necessary, currently not checking for overflow in the addition
A_Sterner 6:96d4e0fa0827 178
A_Sterner 6:96d4e0fa0827 179 if(abs(SaturateLimit((Kp_L*error_L+Ki_L*int_L)/14,1))<1){
A_Sterner 6:96d4e0fa0827 180 int_L = int_L + error_L;}
JordanWisdom 0:4c63f2192998 181 else{
A_Sterner 6:96d4e0fa0827 182 int_L = int_L;
JordanWisdom 0:4c63f2192998 183 }
JordanWisdom 0:4c63f2192998 184
A_Sterner 6:96d4e0fa0827 185 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
A_Sterner 6:96d4e0fa0827 186 int_R = int_R + error_R;}
JordanWisdom 0:4c63f2192998 187 else{
A_Sterner 6:96d4e0fa0827 188 int_R = int_R;
JordanWisdom 0:4c63f2192998 189 }
A_Sterner 6:96d4e0fa0827 190
A_Sterner 6:96d4e0fa0827 191 amp_L = SaturateLimit((Kp_L*error_L + Ki_L*int_L),1);
A_Sterner 6:96d4e0fa0827 192 amp_R = SaturateLimit((Kp_R*error_R + Ki_R*int_R),1);
A_Sterner 6:96d4e0fa0827 193
JordanWisdom 5:ac5819613d0c 194 //Determine direction bits based on sign of required output signal
A_Sterner 6:96d4e0fa0827 195
A_Sterner 6:96d4e0fa0827 196 if(amp_L <=0)
JordanWisdom 0:4c63f2192998 197 DirL = 0;
JordanWisdom 0:4c63f2192998 198 else
JordanWisdom 0:4c63f2192998 199 DirL = 1;
JordanWisdom 0:4c63f2192998 200
A_Sterner 6:96d4e0fa0827 201 if(amp_R <=0)
JordanWisdom 0:4c63f2192998 202 DirR = 1;
JordanWisdom 0:4c63f2192998 203 else
JordanWisdom 0:4c63f2192998 204 DirR = 0;
JordanWisdom 5:ac5819613d0c 205
A_Sterner 6:96d4e0fa0827 206 //Write final values to the PWM
A_Sterner 6:96d4e0fa0827 207
A_Sterner 6:96d4e0fa0827 208 PwmL.write(abs(amp_L));
A_Sterner 6:96d4e0fa0827 209 PwmR.write(abs(amp_R));
JordanWisdom 0:4c63f2192998 210 }
JordanWisdom 0:4c63f2192998 211
A_Sterner 6:96d4e0fa0827 212 /*// *************************************************
A_Sterner 6:96d4e0fa0827 213 // SaturateAdd - Not Used
JordanWisdom 0:4c63f2192998 214 // ***************************************************
JordanWisdom 0:4c63f2192998 215
JordanWisdom 0:4c63f2192998 216 signed int SaturateAdd(signed int x, signed int y){
JordanWisdom 0:4c63f2192998 217
JordanWisdom 0:4c63f2192998 218 signed int z = x + y;
JordanWisdom 0:4c63f2192998 219
JordanWisdom 0:4c63f2192998 220 if( (x>0) && (y>0)&& (z<=0) ){
JordanWisdom 0:4c63f2192998 221 z = 0x7FFFFFFF;}
JordanWisdom 0:4c63f2192998 222
JordanWisdom 0:4c63f2192998 223 else if( (x<0)&&(y<0)&&(z>=0) ){
JordanWisdom 0:4c63f2192998 224 z = 0x80000000;}
JordanWisdom 0:4c63f2192998 225
JordanWisdom 0:4c63f2192998 226 return z;
JordanWisdom 4:1bcb35cef400 227 }*/
JordanWisdom 0:4c63f2192998 228
A_Sterner 6:96d4e0fa0827 229 // ***************************************************
A_Sterner 6:96d4e0fa0827 230 // SaturateLimit - Done, AS - 1/4/16
JordanWisdom 0:4c63f2192998 231 // ***************************************************
JordanWisdom 0:4c63f2192998 232
JordanWisdom 5:ac5819613d0c 233 float SaturateLimit(float x, float limit){ //Ensures we don't ask for more than the PWM can give.
JordanWisdom 0:4c63f2192998 234
JordanWisdom 0:4c63f2192998 235 if (x > limit){
JordanWisdom 0:4c63f2192998 236 return limit;
JordanWisdom 0:4c63f2192998 237 }
JordanWisdom 0:4c63f2192998 238 else if(x < -limit){
JordanWisdom 0:4c63f2192998 239 return(-limit);
JordanWisdom 0:4c63f2192998 240 }
JordanWisdom 0:4c63f2192998 241 else{
JordanWisdom 0:4c63f2192998 242 return x;}
JordanWisdom 0:4c63f2192998 243
JordanWisdom 0:4c63f2192998 244 }
JordanWisdom 0:4c63f2192998 245
JordanWisdom 0:4c63f2192998 246 /// ***************************************************
A_Sterner 6:96d4e0fa0827 247 // SignExtend - Done, AS - 1/4/16
JordanWisdom 0:4c63f2192998 248 // ***************************************************
JordanWisdom 0:4c63f2192998 249
JordanWisdom 0:4c63f2192998 250 signed int SignExtend(int x){
JordanWisdom 0:4c63f2192998 251
JordanWisdom 0:4c63f2192998 252 if(x&0x00008000){
JordanWisdom 0:4c63f2192998 253 x = x|0xFFFF0000;
JordanWisdom 0:4c63f2192998 254 }
JordanWisdom 0:4c63f2192998 255
JordanWisdom 0:4c63f2192998 256 return x;
JordanWisdom 0:4c63f2192998 257 }
JordanWisdom 0:4c63f2192998 258
A_Sterner 6:96d4e0fa0827 259 /// ***************************************************
A_Sterner 6:96d4e0fa0827 260 // SignExtend - Done, AS - 1/4/16
JordanWisdom 4:1bcb35cef400 261 // ***************************************************
JordanWisdom 4:1bcb35cef400 262
A_Sterner 6:96d4e0fa0827 263 void IntegratorInit(void){
JordanWisdom 5:ac5819613d0c 264
A_Sterner 6:96d4e0fa0827 265 int_L = 0;
A_Sterner 6:96d4e0fa0827 266 int_R = 0;
A_Sterner 6:96d4e0fa0827 267
JordanWisdom 4:1bcb35cef400 268 }
JordanWisdom 0:4c63f2192998 269 // *********************************************************************
JordanWisdom 0:4c63f2192998 270 // MAIN FUNCTION
JordanWisdom 0:4c63f2192998 271 // *********************************************************************
JordanWisdom 0:4c63f2192998 272
JordanWisdom 0:4c63f2192998 273 int main(){
A_Sterner 6:96d4e0fa0827 274
JordanWisdom 1:d523415c7b53 275 // Initialization
JordanWisdom 0:4c63f2192998 276
A_Sterner 6:96d4e0fa0827 277 DE0_Init(0x8004);
A_Sterner 6:96d4e0fa0827 278 MotorInit();
A_Sterner 6:96d4e0fa0827 279 ControlInterrupt.attach(&ControlThread, 0.00025);
A_Sterner 6:96d4e0fa0827 280 IntegratorInit();
A_Sterner 6:96d4e0fa0827 281 float dL,dR,v,vR,vL;
A_Sterner 6:96d4e0fa0827 282
A_Sterner 6:96d4e0fa0827 283 //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
A_Sterner 6:96d4e0fa0827 284
A_Sterner 6:96d4e0fa0827 285 float R[5]={30.5,100000,-61,100000,45.7};
A_Sterner 6:96d4e0fa0827 286 float d[5]={47.9*1.05,152.4,287.3,91.4,143.6};
A_Sterner 6:96d4e0fa0827 287
A_Sterner 6:96d4e0fa0827 288 pc.printf("\n\r ready to work");
A_Sterner 6:96d4e0fa0827 289 wait(5);
A_Sterner 6:96d4e0fa0827 290 //int i=0;
A_Sterner 6:96d4e0fa0827 291 for( int i = 0 ; i < 5 ; i++ ){
A_Sterner 6:96d4e0fa0827 292
A_Sterner 6:96d4e0fa0827 293 //pc.printf("\n\r off i go then");
A_Sterner 6:96d4e0fa0827 294 dL = d[i]*(1+L/(2*R[i]))/Rw*(851400/6804)*(100/pi)/1.9;
A_Sterner 6:96d4e0fa0827 295 dR = d[i]*(1-L/(2*R[i]))/Rw*(851400/6804)*(100/pi)/1.9;
A_Sterner 6:96d4e0fa0827 296
A_Sterner 6:96d4e0fa0827 297 v = 25;
A_Sterner 6:96d4e0fa0827 298 setpoint_R = v*(1-L/(2*R[i]))/Rw;
A_Sterner 6:96d4e0fa0827 299 setpoint_L = v*(1+L/(2*R[i]))/Rw;
A_Sterner 6:96d4e0fa0827 300
A_Sterner 6:96d4e0fa0827 301 pc.printf("\n\r%2.2f",d[i]);
A_Sterner 6:96d4e0fa0827 302 pc.printf("\n\r%2.2f",R[i]);
A_Sterner 6:96d4e0fa0827 303 pc.printf("\n\r %2.2f",dL);
A_Sterner 6:96d4e0fa0827 304 pc.printf("\n\r %2.2f",dR);
A_Sterner 6:96d4e0fa0827 305 printf("\n\r%2.2f",setpoint_R);
A_Sterner 6:96d4e0fa0827 306 printf("\n\r%2.2f",setpoint_L);
A_Sterner 6:96d4e0fa0827 307
A_Sterner 6:96d4e0fa0827 308 while((PositionLeft<dL) || (PositionRight<dR)){
A_Sterner 6:96d4e0fa0827 309 pc.printf("\n\r%2.2f, %2.2f",setpoint_L,omega_L);
A_Sterner 6:96d4e0fa0827 310 pc.printf("\n\r%2.2f, %2.2f",setpoint_R,omega_R);
A_Sterner 6:96d4e0fa0827 311 pc.printf("\n\r");
A_Sterner 6:96d4e0fa0827 312 wait(0.1);
A_Sterner 6:96d4e0fa0827 313 }
A_Sterner 6:96d4e0fa0827 314
A_Sterner 6:96d4e0fa0827 315 PositionLeft = 0;
A_Sterner 6:96d4e0fa0827 316 PositionRight = 0;
A_Sterner 6:96d4e0fa0827 317 }
A_Sterner 6:96d4e0fa0827 318 setpoint_R = 0;
A_Sterner 6:96d4e0fa0827 319 setpoint_L = 0;
JordanWisdom 0:4c63f2192998 320 }