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@2:82e4eac97f0a, 2016-02-19 (annotated)

Committer:
A_Sterner
Date:
Fri Feb 19 21:40:48 2016 +0000
Revision:
2:82e4eac97f0a
Parent:
0:b3cd4463d972
Child:
3:30244b9e5351
Child:
4:43aef502bb73
PI Controller

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