ECE 4333 / Mbed 2 deprecated Manual

WASD, R to stop, Q to go straight at same speed

Dependencies:   mbed

main.cpp@1:b09a2f277b93, 2016-04-05 (annotated)

Committer:
A_Sterner
Date:
Tue Apr 05 19:31:29 2016 +0000
Revision:
1:b09a2f277b93
Parent:
0:f8a9fb252057 
WASD,r to stop, q to go straight

Who changed what in which revision?

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