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@4:43aef502bb73, 2016-02-24 (annotated)

Committer:
A_Sterner
Date:
Wed Feb 24 14:44:32 2016 +0000
Revision:
4:43aef502bb73
Parent:
2:82e4eac97f0a
Child:
5:e33e69d4eecf
Right motor code

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 4:43aef502bb73 26 signed int R_setpoint; // Desired Angular Speed ( rad/sec )
A_Sterner 4:43aef502bb73 27 signed int L_setpoint;
A_Sterner 4:43aef502bb73 28 float R_e; // Velocity Error
A_Sterner 4:43aef502bb73 29 float R_u; // Control Signal
A_Sterner 4:43aef502bb73 30 float L_e;
A_Sterner 4:43aef502bb73 31 float L_u;
A_Sterner 2:82e4eac97f0a 32 int L_integrator; // Left Integrator State
A_Sterner 4:43aef502bb73 33 int R_integrator;
A_Sterner 2:82e4eac97f0a 34 signed int dPositionLeft; // DE0 Register 0
A_Sterner 4:43aef502bb73 35 signed int dPositionRight;
A_Sterner 2:82e4eac97f0a 36 int dTimeLeft; // DE0 Register 1
A_Sterner 4:43aef502bb73 37 int dTimeRight;
A_Sterner 2:82e4eac97f0a 38
A_Sterner 0:b3cd4463d972 39 // *********************************************************************
A_Sterner 0:b3cd4463d972 40 // PROCESSES AND THREADS
A_Sterner 0:b3cd4463d972 41 // *********************************************************************
A_Sterner 0:b3cd4463d972 42
A_Sterner 0:b3cd4463d972 43
A_Sterner 0:b3cd4463d972 44 // *********************************************************************
A_Sterner 0:b3cd4463d972 45 // PIN DECLARATIONS
A_Sterner 0:b3cd4463d972 46 // *********************************************************************
A_Sterner 0:b3cd4463d972 47
A_Sterner 2:82e4eac97f0a 48 // Digital I/O Pins
A_Sterner 0:b3cd4463d972 49
A_Sterner 0:b3cd4463d972 50 DigitalOut led1(LED1); // Thread Indicators
A_Sterner 0:b3cd4463d972 51 DigitalOut led2(LED2); //
A_Sterner 0:b3cd4463d972 52 DigitalOut led3(LED3); //
A_Sterner 0:b3cd4463d972 53 DigitalOut led4(LED4); //
A_Sterner 0:b3cd4463d972 54
A_Sterner 0:b3cd4463d972 55 DigitalOut DirL(p29); // Direction of Left Motor
A_Sterner 0:b3cd4463d972 56 DigitalOut DirR(p30); // Direction of Right Motor
A_Sterner 0:b3cd4463d972 57
A_Sterner 0:b3cd4463d972 58 // SPI Related Digital I/O Pins
A_Sterner 0:b3cd4463d972 59
A_Sterner 0:b3cd4463d972 60 DigitalOut SpiReset(p11);
A_Sterner 0:b3cd4463d972 61 DigitalOut IoReset(p12);
A_Sterner 0:b3cd4463d972 62
A_Sterner 2:82e4eac97f0a 63 //PWM
A_Sterner 0:b3cd4463d972 64
A_Sterner 0:b3cd4463d972 65 PwmOut PwmL(p22);
A_Sterner 0:b3cd4463d972 66 PwmOut PwmR(p21);
A_Sterner 0:b3cd4463d972 67
A_Sterner 2:82e4eac97f0a 68 //Serial
A_Sterner 0:b3cd4463d972 69
A_Sterner 0:b3cd4463d972 70 Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable
A_Sterner 0:b3cd4463d972 71 Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel
A_Sterner 0:b3cd4463d972 72
A_Sterner 2:82e4eac97f0a 73 //SPI
A_Sterner 0:b3cd4463d972 74
A_Sterner 0:b3cd4463d972 75 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
A_Sterner 2:82e4eac97f0a 76
A_Sterner 2:82e4eac97f0a 77 //Interrupts
A_Sterner 2:82e4eac97f0a 78
A_Sterner 0:b3cd4463d972 79 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
A_Sterner 0:b3cd4463d972 80
A_Sterner 0:b3cd4463d972 81
A_Sterner 0:b3cd4463d972 82 // ***************************************************
A_Sterner 0:b3cd4463d972 83 // DE0 Init
A_Sterner 0:b3cd4463d972 84 // ***************************************************
A_Sterner 0:b3cd4463d972 85
A_Sterner 0:b3cd4463d972 86 void DE0_Init(int SpiControlWord){
A_Sterner 0:b3cd4463d972 87
A_Sterner 0:b3cd4463d972 88 int mode = 1;
A_Sterner 0:b3cd4463d972 89 int bits = 16;
A_Sterner 0:b3cd4463d972 90
A_Sterner 0:b3cd4463d972 91 DE0.format(bits,mode);
A_Sterner 0:b3cd4463d972 92
A_Sterner 0:b3cd4463d972 93 // Verify Peripheral ID
A_Sterner 0:b3cd4463d972 94
A_Sterner 0:b3cd4463d972 95 // Generates single square pulse to reset DE0 IO
A_Sterner 0:b3cd4463d972 96
A_Sterner 0:b3cd4463d972 97 IoReset = 0;
A_Sterner 0:b3cd4463d972 98 IoReset = 1;
A_Sterner 0:b3cd4463d972 99 IoReset = 0;
A_Sterner 0:b3cd4463d972 100
A_Sterner 0:b3cd4463d972 101 // Generates single square pulse to reset DE0 SPI
A_Sterner 0:b3cd4463d972 102
A_Sterner 0:b3cd4463d972 103 SpiReset = 0;
A_Sterner 0:b3cd4463d972 104 SpiReset = 1;
A_Sterner 0:b3cd4463d972 105 SpiReset = 0;
A_Sterner 0:b3cd4463d972 106
A_Sterner 0:b3cd4463d972 107 // Writes to DE0 Control Register
A_Sterner 0:b3cd4463d972 108
A_Sterner 0:b3cd4463d972 109 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
A_Sterner 0:b3cd4463d972 110
A_Sterner 0:b3cd4463d972 111 if(ID == 23){ // DE0 ID 23 (0x0017)
A_Sterner 0:b3cd4463d972 112 printf("\n\r >> DE0 Initialized.\n\r");}
A_Sterner 0:b3cd4463d972 113 else{
A_Sterner 0:b3cd4463d972 114 printf("\n\r >> Failed to initialize DE0 board.\n\r");}
A_Sterner 0:b3cd4463d972 115 }
A_Sterner 0:b3cd4463d972 116
A_Sterner 0:b3cd4463d972 117 // ***************************************************
A_Sterner 0:b3cd4463d972 118 // Left Motor Initialization
A_Sterner 0:b3cd4463d972 119 // ***************************************************
A_Sterner 0:b3cd4463d972 120
A_Sterner 0:b3cd4463d972 121 // Pwm Pin Left Motor : p21
A_Sterner 0:b3cd4463d972 122 // Direction Pin Left Motor : p29
A_Sterner 0:b3cd4463d972 123
A_Sterner 0:b3cd4463d972 124 void L_MotorInit(void){
A_Sterner 0:b3cd4463d972 125
A_Sterner 4:43aef502bb73 126 DirL = 1; // Defaults to 1
A_Sterner 0:b3cd4463d972 127
A_Sterner 0:b3cd4463d972 128 // Direction bit logic output
A_Sterner 0:b3cd4463d972 129 // 0 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 130 // 1 : Forwards ( Advance )
A_Sterner 0:b3cd4463d972 131
A_Sterner 0:b3cd4463d972 132 PwmL.period_us(100);
A_Sterner 4:43aef502bb73 133 PwmL.write(0);
A_Sterner 0:b3cd4463d972 134
A_Sterner 0:b3cd4463d972 135 }
A_Sterner 0:b3cd4463d972 136
A_Sterner 0:b3cd4463d972 137 // ***************************************************
A_Sterner 0:b3cd4463d972 138 // Right Motor Initialization
A_Sterner 0:b3cd4463d972 139 // ***************************************************
A_Sterner 0:b3cd4463d972 140
A_Sterner 0:b3cd4463d972 141 // Pwm Pin Right Motor : p22
A_Sterner 0:b3cd4463d972 142 // Direction Pin Right Motor : p30
A_Sterner 0:b3cd4463d972 143
A_Sterner 0:b3cd4463d972 144 void R_MotorInit(void){
A_Sterner 0:b3cd4463d972 145
A_Sterner 4:43aef502bb73 146 DirR = 0; // Defaults to 0.
A_Sterner 0:b3cd4463d972 147
A_Sterner 0:b3cd4463d972 148 // Direction bit logic output
A_Sterner 2:82e4eac97f0a 149 // 0 : Forwards ( Advance )
A_Sterner 2:82e4eac97f0a 150 // 1 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 151
A_Sterner 0:b3cd4463d972 152 PwmR.period_us(100);
A_Sterner 2:82e4eac97f0a 153 PwmR.write(0);
A_Sterner 0:b3cd4463d972 154
A_Sterner 0:b3cd4463d972 155 }
A_Sterner 0:b3cd4463d972 156
A_Sterner 0:b3cd4463d972 157 /// ***************************************************
A_Sterner 0:b3cd4463d972 158 // User Input
A_Sterner 0:b3cd4463d972 159 // ***************************************************
A_Sterner 0:b3cd4463d972 160
A_Sterner 2:82e4eac97f0a 161 signed int UserInput(void){
A_Sterner 0:b3cd4463d972 162
A_Sterner 2:82e4eac97f0a 163 signed int input;
A_Sterner 0:b3cd4463d972 164
A_Sterner 2:82e4eac97f0a 165 printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
A_Sterner 2:82e4eac97f0a 166 scanf("%i",&input);
A_Sterner 2:82e4eac97f0a 167 printf("\n\r Your number was >> %i\n\r",input);
A_Sterner 0:b3cd4463d972 168
A_Sterner 2:82e4eac97f0a 169 return input;
A_Sterner 0:b3cd4463d972 170
A_Sterner 0:b3cd4463d972 171 }
A_Sterner 0:b3cd4463d972 172
A_Sterner 0:b3cd4463d972 173
A_Sterner 2:82e4eac97f0a 174 /// ***************************************************
A_Sterner 2:82e4eac97f0a 175 // Control Thread
A_Sterner 2:82e4eac97f0a 176 // ***************************************************
A_Sterner 2:82e4eac97f0a 177
A_Sterner 2:82e4eac97f0a 178 void ControlThread(void){
A_Sterner 2:82e4eac97f0a 179
A_Sterner 2:82e4eac97f0a 180 // Read Incremental Position from DE0 QEI
A_Sterner 0:b3cd4463d972 181
A_Sterner 2:82e4eac97f0a 182 int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register
A_Sterner 2:82e4eac97f0a 183
A_Sterner 2:82e4eac97f0a 184 dPositionLeft = SignExtend(DE0.write(dummy));
A_Sterner 2:82e4eac97f0a 185 dTimeLeft = DE0.write(dummy);
A_Sterner 4:43aef502bb73 186 dPositionRight = SignExtend(DE0.write(dummy));
A_Sterner 4:43aef502bb73 187 dTimeRight = DE0.write(dummy);
A_Sterner 2:82e4eac97f0a 188
A_Sterner 2:82e4eac97f0a 189 // Computer Angular Speed and Angular Speed Error
A_Sterner 2:82e4eac97f0a 190
A_Sterner 2:82e4eac97f0a 191 signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
A_Sterner 4:43aef502bb73 192 signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
A_Sterner 2:82e4eac97f0a 193
A_Sterner 4:43aef502bb73 194 L_e = L_setpoint - AngularSpeedLeft;
A_Sterner 4:43aef502bb73 195 R_e = R_setpoint - AngularSpeedRight;
A_Sterner 4:43aef502bb73 196
A_Sterner 4:43aef502bb73 197 float Kp_L = 2.5;
A_Sterner 4:43aef502bb73 198 float Ki_L = 0.010;
A_Sterner 2:82e4eac97f0a 199
A_Sterner 4:43aef502bb73 200
A_Sterner 4:43aef502bb73 201 float Kp_R = 2.5;
A_Sterner 4:43aef502bb73 202 float Ki_R = 0.010;
A_Sterner 2:82e4eac97f0a 203
A_Sterner 4:43aef502bb73 204
A_Sterner 4:43aef502bb73 205 if(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1)<1){
A_Sterner 4:43aef502bb73 206 L_integrator = L_integrator +L_e;}
A_Sterner 2:82e4eac97f0a 207 else{
A_Sterner 2:82e4eac97f0a 208 L_integrator = L_integrator;
A_Sterner 0:b3cd4463d972 209 }
A_Sterner 2:82e4eac97f0a 210
A_Sterner 4:43aef502bb73 211 if(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1)<1){
A_Sterner 4:43aef502bb73 212 R_integrator = R_integrator +R_e;}
A_Sterner 4:43aef502bb73 213 else{
A_Sterner 4:43aef502bb73 214 R_integrator = R_integrator;
A_Sterner 4:43aef502bb73 215 }
A_Sterner 2:82e4eac97f0a 216
A_Sterner 4:43aef502bb73 217
A_Sterner 4:43aef502bb73 218 L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
A_Sterner 4:43aef502bb73 219 R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
A_Sterner 4:43aef502bb73 220
A_Sterner 4:43aef502bb73 221 PwmL.write(L_u);
A_Sterner 4:43aef502bb73 222 PwmR.write(R_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
A_Sterner 4:43aef502bb73 286 DE0_Init(0x8004);
A_Sterner 2:82e4eac97f0a 287 L_MotorInit();
A_Sterner 4:43aef502bb73 288 R_MotorInit();
A_Sterner 2:82e4eac97f0a 289 L_integrator = 0;
A_Sterner 4:43aef502bb73 290 R_integrator = 0;
A_Sterner 2:82e4eac97f0a 291 ControlInterrupt.attach(&ControlThread, 0.0005);
A_Sterner 2:82e4eac97f0a 292
A_Sterner 2:82e4eac97f0a 293 // Specify Setpoint ( rads/sec )
A_Sterner 2:82e4eac97f0a 294
A_Sterner 4:43aef502bb73 295 L_setpoint = UserInput();
A_Sterner 4:43aef502bb73 296 R_setpoint = UserInput();
A_Sterner 4:43aef502bb73 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
A_Sterner 4:43aef502bb73 302 float L_error_t = L_e;
A_Sterner 4:43aef502bb73 303 float L_u_t = L_u;
A_Sterner 4:43aef502bb73 304
A_Sterner 4:43aef502bb73 305 float R_error_t = R_e;
A_Sterner 4:43aef502bb73 306 float R_u_t = R_u;
A_Sterner 2:82e4eac97f0a 307
A_Sterner 4:43aef502bb73 308 printf("\n\r LEFT");
A_Sterner 4:43aef502bb73 309 printf("\n\r US : %i",L_setpoint); // User defined setpoint
A_Sterner 4:43aef502bb73 310 printf("\n\r VE : %2.2f",L_error_t); // Displays signed Velocity Error
A_Sterner 2:82e4eac97f0a 311 printf("\n\r IS : %i",L_integrator); // Displays currently state of the left integrator
A_Sterner 4:43aef502bb73 312 printf("\n\r CS : %2.4f",L_u_t); // Displays control signal
A_Sterner 4:43aef502bb73 313 printf("\n\r RIGHT");
A_Sterner 4:43aef502bb73 314 printf("\n\r US : %i",R_setpoint); // User defined setpoint
A_Sterner 4:43aef502bb73 315 printf("\n\r VE : %2.2f",R_error_t); // Displays signed Velocity Error
A_Sterner 4:43aef502bb73 316 printf("\n\r IS : %i",R_integrator); // Displays currently state of the left integrator
A_Sterner 4:43aef502bb73 317 printf("\n\r CS : %2.4f",R_u_t); // Displays control signal
A_Sterner 2:82e4eac97f0a 318 printf("\n\r\n\r");
A_Sterner 2:82e4eac97f0a 319 wait(0.75);
A_Sterner 2:82e4eac97f0a 320 }
A_Sterner 2:82e4eac97f0a 321
A_Sterner 2:82e4eac97f0a 322 }