ECE 4333 / Mbed 2 deprecated Lab3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Lab3.cpp@3:30244b9e5351, 2016-02-20 (annotated)

Committer:
JordanWisdom
Date:
Sat Feb 20 15:20:03 2016 +0000
Revision:
3:30244b9e5351
Parent:
2:82e4eac97f0a 
Right Motor Functions added. ; System sets the left motor, then the right. That way the effects of the left motor on the right one are mitigated by the existing control system.; Left and Right motor controls are separate variables.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
A_Sterner 0:b3cd4463d972 1 // EE4333 Robotics Lab 3
A_Sterner 0:b3cd4463d972 2
A_Sterner 0:b3cd4463d972 3 // Library Imports
A_Sterner 0:b3cd4463d972 4
A_Sterner 0:b3cd4463d972 5 //#include "InterruptIn.h"
A_Sterner 0:b3cd4463d972 6 //#include "rtos.h"
A_Sterner 0:b3cd4463d972 7 #include "mbed.h"
A_Sterner 0:b3cd4463d972 8 #include "Serial.h"
A_Sterner 0:b3cd4463d972 9 #include "stdio.h"
A_Sterner 0:b3cd4463d972 10
A_Sterner 0:b3cd4463d972 11 // Function Declarations
A_Sterner 0:b3cd4463d972 12
A_Sterner 2:82e4eac97f0a 13 void DE0_Init(int);
A_Sterner 2:82e4eac97f0a 14 void L_MotorInit(void);
A_Sterner 2:82e4eac97f0a 15 void R_MotorInit(void);
A_Sterner 2:82e4eac97f0a 16 signed int UserInput(void);
A_Sterner 2:82e4eac97f0a 17 void ControlThread(void);
A_Sterner 2:82e4eac97f0a 18 int SaturateAdd(int x, int y);
A_Sterner 2:82e4eac97f0a 19 float SaturateLimit(float x, float limit);
A_Sterner 2:82e4eac97f0a 20 signed int SignExtend(signed int x);
A_Sterner 0:b3cd4463d972 21
A_Sterner 0:b3cd4463d972 22 // ********************************************************************
A_Sterner 0:b3cd4463d972 23 // GLOBAL VARIABLE DECLARATIONS
A_Sterner 0:b3cd4463d972 24 // ********************************************************************
A_Sterner 0:b3cd4463d972 25
A_Sterner 0:b3cd4463d972 26 signed int setpoint; // Desired Angular Speed ( rad/sec )
A_Sterner 2:82e4eac97f0a 27 float e; // Velocity Error
A_Sterner 2:82e4eac97f0a 28 float u; // Control Signal
JordanWisdom 3:30244b9e5351 29 float e_r; // Velocity Error - Right Motor
JordanWisdom 3:30244b9e5351 30 float u_r; // Control Signal - Right Motor
A_Sterner 2:82e4eac97f0a 31 int L_integrator; // Left Integrator State
JordanWisdom 3:30244b9e5351 32 int R_integrator; // Right Integrator State
A_Sterner 2:82e4eac97f0a 33 signed int dPositionLeft; // DE0 Register 0
A_Sterner 2:82e4eac97f0a 34 int dTimeLeft; // DE0 Register 1
JordanWisdom 3:30244b9e5351 35 signed int dPositionRight; // DE0 Register 2
JordanWisdom 3:30244b9e5351 36 int dTimeRight; // DE0 Register 3
A_Sterner 2:82e4eac97f0a 37
A_Sterner 0:b3cd4463d972 38 // *********************************************************************
A_Sterner 0:b3cd4463d972 39 // PROCESSES AND THREADS
A_Sterner 0:b3cd4463d972 40 // *********************************************************************
A_Sterner 0:b3cd4463d972 41
A_Sterner 0:b3cd4463d972 42
A_Sterner 0:b3cd4463d972 43 // *********************************************************************
A_Sterner 0:b3cd4463d972 44 // PIN DECLARATIONS
A_Sterner 0:b3cd4463d972 45 // *********************************************************************
A_Sterner 0:b3cd4463d972 46
A_Sterner 2:82e4eac97f0a 47 // Digital I/O Pins
A_Sterner 0:b3cd4463d972 48
A_Sterner 0:b3cd4463d972 49 DigitalOut led1(LED1); // Thread Indicators
A_Sterner 0:b3cd4463d972 50 DigitalOut led2(LED2); //
A_Sterner 0:b3cd4463d972 51 DigitalOut led3(LED3); //
A_Sterner 0:b3cd4463d972 52 DigitalOut led4(LED4); //
A_Sterner 0:b3cd4463d972 53
A_Sterner 0:b3cd4463d972 54 DigitalOut DirL(p29); // Direction of Left Motor
A_Sterner 0:b3cd4463d972 55 DigitalOut DirR(p30); // Direction of Right Motor
A_Sterner 0:b3cd4463d972 56
A_Sterner 0:b3cd4463d972 57 // SPI Related Digital I/O Pins
A_Sterner 0:b3cd4463d972 58
A_Sterner 0:b3cd4463d972 59 DigitalOut SpiReset(p11);
A_Sterner 0:b3cd4463d972 60 DigitalOut IoReset(p12);
A_Sterner 0:b3cd4463d972 61
A_Sterner 2:82e4eac97f0a 62 //PWM
A_Sterner 0:b3cd4463d972 63
A_Sterner 0:b3cd4463d972 64 PwmOut PwmL(p22);
A_Sterner 0:b3cd4463d972 65 PwmOut PwmR(p21);
A_Sterner 0:b3cd4463d972 66
A_Sterner 2:82e4eac97f0a 67 //Serial
A_Sterner 0:b3cd4463d972 68
A_Sterner 0:b3cd4463d972 69 Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable
A_Sterner 0:b3cd4463d972 70 Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel
A_Sterner 0:b3cd4463d972 71
A_Sterner 2:82e4eac97f0a 72 //SPI
A_Sterner 0:b3cd4463d972 73
A_Sterner 0:b3cd4463d972 74 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
A_Sterner 2:82e4eac97f0a 75
A_Sterner 2:82e4eac97f0a 76 //Interrupts
A_Sterner 2:82e4eac97f0a 77
A_Sterner 0:b3cd4463d972 78 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
A_Sterner 0:b3cd4463d972 79
A_Sterner 0:b3cd4463d972 80
A_Sterner 0:b3cd4463d972 81 // ***************************************************
A_Sterner 0:b3cd4463d972 82 // DE0 Init
A_Sterner 0:b3cd4463d972 83 // ***************************************************
A_Sterner 0:b3cd4463d972 84
A_Sterner 0:b3cd4463d972 85 void DE0_Init(int SpiControlWord){
A_Sterner 0:b3cd4463d972 86
A_Sterner 0:b3cd4463d972 87 int mode = 1;
A_Sterner 0:b3cd4463d972 88 int bits = 16;
A_Sterner 0:b3cd4463d972 89
A_Sterner 0:b3cd4463d972 90 DE0.format(bits,mode);
A_Sterner 0:b3cd4463d972 91
A_Sterner 0:b3cd4463d972 92 // Verify Peripheral ID
A_Sterner 0:b3cd4463d972 93
A_Sterner 0:b3cd4463d972 94 // Generates single square pulse to reset DE0 IO
A_Sterner 0:b3cd4463d972 95
A_Sterner 0:b3cd4463d972 96 IoReset = 0;
A_Sterner 0:b3cd4463d972 97 IoReset = 1;
A_Sterner 0:b3cd4463d972 98 IoReset = 0;
A_Sterner 0:b3cd4463d972 99
A_Sterner 0:b3cd4463d972 100 // Generates single square pulse to reset DE0 SPI
A_Sterner 0:b3cd4463d972 101
A_Sterner 0:b3cd4463d972 102 SpiReset = 0;
A_Sterner 0:b3cd4463d972 103 SpiReset = 1;
A_Sterner 0:b3cd4463d972 104 SpiReset = 0;
A_Sterner 0:b3cd4463d972 105
A_Sterner 0:b3cd4463d972 106 // Writes to DE0 Control Register
A_Sterner 0:b3cd4463d972 107
A_Sterner 0:b3cd4463d972 108 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
A_Sterner 0:b3cd4463d972 109
A_Sterner 0:b3cd4463d972 110 if(ID == 23){ // DE0 ID 23 (0x0017)
A_Sterner 0:b3cd4463d972 111 printf("\n\r >> DE0 Initialized.\n\r");}
A_Sterner 0:b3cd4463d972 112 else{
A_Sterner 0:b3cd4463d972 113 printf("\n\r >> Failed to initialize DE0 board.\n\r");}
A_Sterner 0:b3cd4463d972 114 }
A_Sterner 0:b3cd4463d972 115
A_Sterner 0:b3cd4463d972 116 // ***************************************************
A_Sterner 0:b3cd4463d972 117 // Left Motor Initialization
A_Sterner 0:b3cd4463d972 118 // ***************************************************
A_Sterner 0:b3cd4463d972 119
A_Sterner 0:b3cd4463d972 120 // Pwm Pin Left Motor : p21
A_Sterner 0:b3cd4463d972 121 // Direction Pin Left Motor : p29
A_Sterner 0:b3cd4463d972 122
A_Sterner 0:b3cd4463d972 123 void L_MotorInit(void){
A_Sterner 0:b3cd4463d972 124
A_Sterner 0:b3cd4463d972 125 DirL = 1; // Defaults to 0.
A_Sterner 0:b3cd4463d972 126
A_Sterner 0:b3cd4463d972 127 // Direction bit logic output
A_Sterner 0:b3cd4463d972 128 // 0 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 129 // 1 : Forwards ( Advance )
A_Sterner 0:b3cd4463d972 130
A_Sterner 0:b3cd4463d972 131 PwmL.period_us(100);
A_Sterner 2:82e4eac97f0a 132 PwmL.write(0.7);
A_Sterner 0:b3cd4463d972 133
A_Sterner 0:b3cd4463d972 134 }
A_Sterner 0:b3cd4463d972 135
A_Sterner 0:b3cd4463d972 136 // ***************************************************
A_Sterner 0:b3cd4463d972 137 // Right Motor Initialization
A_Sterner 0:b3cd4463d972 138 // ***************************************************
A_Sterner 0:b3cd4463d972 139
A_Sterner 0:b3cd4463d972 140 // Pwm Pin Right Motor : p22
A_Sterner 0:b3cd4463d972 141 // Direction Pin Right Motor : p30
A_Sterner 0:b3cd4463d972 142
A_Sterner 0:b3cd4463d972 143 void R_MotorInit(void){
A_Sterner 0:b3cd4463d972 144
A_Sterner 0:b3cd4463d972 145 DirR = 1; // Defaults to 0.
A_Sterner 0:b3cd4463d972 146
A_Sterner 0:b3cd4463d972 147 // Direction bit logic output
A_Sterner 2:82e4eac97f0a 148 // 0 : Forwards ( Advance )
A_Sterner 2:82e4eac97f0a 149 // 1 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 150
A_Sterner 0:b3cd4463d972 151 PwmR.period_us(100);
A_Sterner 2:82e4eac97f0a 152 PwmR.write(0);
A_Sterner 0:b3cd4463d972 153
A_Sterner 0:b3cd4463d972 154 }
A_Sterner 0:b3cd4463d972 155
A_Sterner 0:b3cd4463d972 156 /// ***************************************************
A_Sterner 0:b3cd4463d972 157 // User Input
A_Sterner 0:b3cd4463d972 158 // ***************************************************
A_Sterner 0:b3cd4463d972 159
A_Sterner 2:82e4eac97f0a 160 signed int UserInput(void){
A_Sterner 0:b3cd4463d972 161
A_Sterner 2:82e4eac97f0a 162 signed int input;
A_Sterner 0:b3cd4463d972 163
A_Sterner 2:82e4eac97f0a 164 printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
A_Sterner 2:82e4eac97f0a 165 scanf("%i",&input);
A_Sterner 2:82e4eac97f0a 166 printf("\n\r Your number was >> %i\n\r",input);
A_Sterner 0:b3cd4463d972 167
A_Sterner 2:82e4eac97f0a 168 return input;
A_Sterner 0:b3cd4463d972 169
A_Sterner 0:b3cd4463d972 170 }
A_Sterner 0:b3cd4463d972 171
A_Sterner 0:b3cd4463d972 172
A_Sterner 2:82e4eac97f0a 173 /// ***************************************************
A_Sterner 2:82e4eac97f0a 174 // Control Thread
A_Sterner 2:82e4eac97f0a 175 // ***************************************************
A_Sterner 2:82e4eac97f0a 176
A_Sterner 2:82e4eac97f0a 177 void ControlThread(void){
A_Sterner 2:82e4eac97f0a 178
JordanWisdom 3:30244b9e5351 179 // *********** LEFT MOTOR CONTROL
JordanWisdom 3:30244b9e5351 180
A_Sterner 2:82e4eac97f0a 181 // Read Incremental Position from DE0 QEI
A_Sterner 0:b3cd4463d972 182
A_Sterner 2:82e4eac97f0a 183 int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register
A_Sterner 2:82e4eac97f0a 184
A_Sterner 2:82e4eac97f0a 185 dPositionLeft = SignExtend(DE0.write(dummy));
A_Sterner 2:82e4eac97f0a 186 dTimeLeft = DE0.write(dummy);
JordanWisdom 3:30244b9e5351 187 dPositionRight = SignExtend(DE0.write(dummy));
JordanWisdom 3:30244b9e5351 188 dTimeRight = DE0.write(dummy);
A_Sterner 2:82e4eac97f0a 189
A_Sterner 2:82e4eac97f0a 190 // Computer Angular Speed and Angular Speed Error
A_Sterner 2:82e4eac97f0a 191
A_Sterner 2:82e4eac97f0a 192 signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
A_Sterner 2:82e4eac97f0a 193 e = setpoint - AngularSpeedLeft;
A_Sterner 2:82e4eac97f0a 194
A_Sterner 2:82e4eac97f0a 195 float Kp = 2.5;
A_Sterner 2:82e4eac97f0a 196 float Ki = 0.010;
A_Sterner 2:82e4eac97f0a 197
A_Sterner 2:82e4eac97f0a 198 if(SaturateLimit((Kp*e+Ki*L_integrator)/35,1)<1){
A_Sterner 2:82e4eac97f0a 199 L_integrator = L_integrator +e;}
A_Sterner 2:82e4eac97f0a 200 else{
A_Sterner 2:82e4eac97f0a 201 L_integrator = L_integrator;
A_Sterner 0:b3cd4463d972 202 }
A_Sterner 2:82e4eac97f0a 203
JordanWisdom 3:30244b9e5351 204 // *********** RIGHT MOTOR CONTROL
A_Sterner 2:82e4eac97f0a 205
A_Sterner 2:82e4eac97f0a 206 u = SaturateLimit( (Kp*e+Ki*L_integrator),1);
A_Sterner 2:82e4eac97f0a 207 PwmL.write(u);
JordanWisdom 3:30244b9e5351 208
JordanWisdom 3:30244b9e5351 209 signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
JordanWisdom 3:30244b9e5351 210 e_r = setpoint - AngularSpeedRight;
JordanWisdom 3:30244b9e5351 211
JordanWisdom 3:30244b9e5351 212 float Kp_r = 1;
JordanWisdom 3:30244b9e5351 213 float Ki_r = 0;
JordanWisdom 3:30244b9e5351 214
JordanWisdom 3:30244b9e5351 215 if(SaturateLimit((Kp_r*e_r+Ki_r*R_integrator)/35,1)<1){
JordanWisdom 3:30244b9e5351 216 R_integrator = R_integrator +e;}
JordanWisdom 3:30244b9e5351 217 else{
JordanWisdom 3:30244b9e5351 218 R_integrator = R_integrator;
JordanWisdom 3:30244b9e5351 219 }
JordanWisdom 3:30244b9e5351 220
JordanWisdom 3:30244b9e5351 221 u_r = SaturateLimit( (Kp_r*e+Ki_r*R_integrator),1);
JordanWisdom 3:30244b9e5351 222 PwmR.write(u);
A_Sterner 0:b3cd4463d972 223 }
A_Sterner 0:b3cd4463d972 224
A_Sterner 2:82e4eac97f0a 225 /// ***************************************************
A_Sterner 2:82e4eac97f0a 226 // SaturateAdd
A_Sterner 2:82e4eac97f0a 227 // ***************************************************
A_Sterner 2:82e4eac97f0a 228
A_Sterner 2:82e4eac97f0a 229 signed int SaturateAdd(signed int x, signed int y){
A_Sterner 2:82e4eac97f0a 230
A_Sterner 2:82e4eac97f0a 231 signed int z = x + y;
A_Sterner 2:82e4eac97f0a 232
A_Sterner 2:82e4eac97f0a 233 if( (x>0) && (y>0)&& (z<=0) ){
A_Sterner 2:82e4eac97f0a 234 z = 0x7FFFFFFF;}
A_Sterner 2:82e4eac97f0a 235
A_Sterner 2:82e4eac97f0a 236 else if( (x<0)&&(y<0)&&(z>=0) ){
A_Sterner 2:82e4eac97f0a 237 z = 0x80000000;}
A_Sterner 2:82e4eac97f0a 238
A_Sterner 2:82e4eac97f0a 239 return z;
A_Sterner 2:82e4eac97f0a 240 }
A_Sterner 2:82e4eac97f0a 241
A_Sterner 2:82e4eac97f0a 242 /// ***************************************************
A_Sterner 2:82e4eac97f0a 243 // SaturateLimit
A_Sterner 2:82e4eac97f0a 244 // ***************************************************
A_Sterner 2:82e4eac97f0a 245
A_Sterner 2:82e4eac97f0a 246 float SaturateLimit(float x, float limit){
A_Sterner 2:82e4eac97f0a 247
A_Sterner 2:82e4eac97f0a 248 if (x > limit){
A_Sterner 2:82e4eac97f0a 249 return limit;}
A_Sterner 2:82e4eac97f0a 250
A_Sterner 2:82e4eac97f0a 251 else if(x < -limit){
A_Sterner 2:82e4eac97f0a 252 return(-limit);}
A_Sterner 2:82e4eac97f0a 253
A_Sterner 2:82e4eac97f0a 254 else{
A_Sterner 2:82e4eac97f0a 255 return x;}
A_Sterner 2:82e4eac97f0a 256
A_Sterner 2:82e4eac97f0a 257 }
A_Sterner 2:82e4eac97f0a 258
A_Sterner 2:82e4eac97f0a 259 /// ***************************************************
A_Sterner 2:82e4eac97f0a 260 // Sign Extend
A_Sterner 2:82e4eac97f0a 261 // ***************************************************
A_Sterner 2:82e4eac97f0a 262
A_Sterner 2:82e4eac97f0a 263 signed int SignExtend(int x){
A_Sterner 2:82e4eac97f0a 264
A_Sterner 2:82e4eac97f0a 265 if(x&0x00008000){
A_Sterner 2:82e4eac97f0a 266 x = x|0xFFFF0000;
A_Sterner 2:82e4eac97f0a 267 }
A_Sterner 2:82e4eac97f0a 268
A_Sterner 2:82e4eac97f0a 269 return x;
A_Sterner 2:82e4eac97f0a 270 }
A_Sterner 2:82e4eac97f0a 271
A_Sterner 2:82e4eac97f0a 272
A_Sterner 2:82e4eac97f0a 273
A_Sterner 2:82e4eac97f0a 274 // ==============================================================================================================
A_Sterner 2:82e4eac97f0a 275 // ==============================================================================================================
A_Sterner 2:82e4eac97f0a 276
A_Sterner 2:82e4eac97f0a 277
A_Sterner 2:82e4eac97f0a 278 // *********************************************************************
A_Sterner 2:82e4eac97f0a 279 // MAIN FUNCTION
A_Sterner 2:82e4eac97f0a 280 // *********************************************************************
A_Sterner 2:82e4eac97f0a 281
A_Sterner 2:82e4eac97f0a 282 int main(){
A_Sterner 2:82e4eac97f0a 283
A_Sterner 2:82e4eac97f0a 284 // Initialization
A_Sterner 2:82e4eac97f0a 285
JordanWisdom 3:30244b9e5351 286 DE0_Init(0x8004); //Initialize for 4 registers
A_Sterner 2:82e4eac97f0a 287 L_MotorInit();
A_Sterner 2:82e4eac97f0a 288 L_integrator = 0;
JordanWisdom 3:30244b9e5351 289 R_MotorInit();
JordanWisdom 3:30244b9e5351 290 R_integrator = 0;
A_Sterner 2:82e4eac97f0a 291 ControlInterrupt.attach(&ControlThread, 0.0005);
A_Sterner 2:82e4eac97f0a 292
A_Sterner 2:82e4eac97f0a 293
A_Sterner 2:82e4eac97f0a 294 // Specify Setpoint ( rads/sec )
A_Sterner 2:82e4eac97f0a 295
A_Sterner 2:82e4eac97f0a 296 setpoint = UserInput();
A_Sterner 2:82e4eac97f0a 297
A_Sterner 2:82e4eac97f0a 298 // Display Global Variables to Console
A_Sterner 2:82e4eac97f0a 299
A_Sterner 2:82e4eac97f0a 300 while(1){
A_Sterner 2:82e4eac97f0a 301
JordanWisdom 3:30244b9e5351 302 float error_t = e_r;
JordanWisdom 3:30244b9e5351 303 float u_t = u_r;
A_Sterner 2:82e4eac97f0a 304
A_Sterner 2:82e4eac97f0a 305 printf("\n\r US : %i",setpoint); // User defined setpoint
JordanWisdom 3:30244b9e5351 306 printf("\n\r VE : %2.2f",error_t); // Displays signed Velocity Error of the right motor
JordanWisdom 3:30244b9e5351 307 printf("\n\r IS : %i",R_integrator); // Displays currently state of the right integrator
JordanWisdom 3:30244b9e5351 308 printf("\n\r CS : %2.4f",u_t); // Displays control signal to right motor
A_Sterner 2:82e4eac97f0a 309 printf("\n\r\n\r");
A_Sterner 2:82e4eac97f0a 310 wait(0.75);
A_Sterner 2:82e4eac97f0a 311 }
A_Sterner 2:82e4eac97f0a 312
A_Sterner 2:82e4eac97f0a 313 }