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@8:9609a50ea076, 2016-02-29 (annotated)

Committer:
JordanWisdom
Date:
Mon Feb 29 14:30:29 2016 +0000
Revision:
8:9609a50ea076
Parent:
7:241bde733699 
Updated the code to remove hard coded values. We can now switch between m/s and rads/s by commenting out 3 lines.

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 5:e33e69d4eecf 16 signed int InputLeft(void);
A_Sterner 5:e33e69d4eecf 17 signed int InputRight(void);
A_Sterner 2:82e4eac97f0a 18 void ControlThread(void);
A_Sterner 2:82e4eac97f0a 19 int SaturateAdd(int x, int y);
A_Sterner 2:82e4eac97f0a 20 float SaturateLimit(float x, float limit);
A_Sterner 2:82e4eac97f0a 21 signed int SignExtend(signed int x);
A_Sterner 0:b3cd4463d972 22
A_Sterner 0:b3cd4463d972 23 // ********************************************************************
A_Sterner 0:b3cd4463d972 24 // GLOBAL VARIABLE DECLARATIONS
A_Sterner 0:b3cd4463d972 25 // ********************************************************************
A_Sterner 0:b3cd4463d972 26
A_Sterner 6:1d16cc833e0d 27 signed int R_setpoint; // Desired Angular Speed ( rad/sec )
A_Sterner 6:1d16cc833e0d 28 signed int L_setpoint;
JordanWisdom 8:9609a50ea076 29
JordanWisdom 8:9609a50ea076 30 signed int R_setpoint_m; // Desired Angular Speed ( m/sec )
JordanWisdom 8:9609a50ea076 31 signed int L_setpoint_m;
JordanWisdom 8:9609a50ea076 32
A_Sterner 4:43aef502bb73 33 float R_e; // Velocity Error
A_Sterner 4:43aef502bb73 34 float R_u; // Control Signal
A_Sterner 4:43aef502bb73 35 float L_e;
A_Sterner 4:43aef502bb73 36 float L_u;
A_Sterner 2:82e4eac97f0a 37 int L_integrator; // Left Integrator State
A_Sterner 4:43aef502bb73 38 int R_integrator;
A_Sterner 2:82e4eac97f0a 39 signed int dPositionLeft; // DE0 Register 0
A_Sterner 4:43aef502bb73 40 signed int dPositionRight;
A_Sterner 2:82e4eac97f0a 41 int dTimeLeft; // DE0 Register 1
A_Sterner 4:43aef502bb73 42 int dTimeRight;
A_Sterner 2:82e4eac97f0a 43
A_Sterner 0:b3cd4463d972 44 // *********************************************************************
A_Sterner 0:b3cd4463d972 45 // PROCESSES AND THREADS
A_Sterner 0:b3cd4463d972 46 // *********************************************************************
A_Sterner 0:b3cd4463d972 47
A_Sterner 0:b3cd4463d972 48
A_Sterner 0:b3cd4463d972 49 // *********************************************************************
A_Sterner 0:b3cd4463d972 50 // PIN DECLARATIONS
A_Sterner 0:b3cd4463d972 51 // *********************************************************************
A_Sterner 0:b3cd4463d972 52
A_Sterner 2:82e4eac97f0a 53 // Digital I/O Pins
A_Sterner 0:b3cd4463d972 54
A_Sterner 0:b3cd4463d972 55 DigitalOut led1(LED1); // Thread Indicators
A_Sterner 0:b3cd4463d972 56 DigitalOut led2(LED2); //
A_Sterner 0:b3cd4463d972 57 DigitalOut led3(LED3); //
A_Sterner 0:b3cd4463d972 58 DigitalOut led4(LED4); //
A_Sterner 0:b3cd4463d972 59
A_Sterner 0:b3cd4463d972 60 DigitalOut DirL(p29); // Direction of Left Motor
A_Sterner 0:b3cd4463d972 61 DigitalOut DirR(p30); // Direction of Right Motor
A_Sterner 0:b3cd4463d972 62
A_Sterner 0:b3cd4463d972 63 // SPI Related Digital I/O Pins
A_Sterner 0:b3cd4463d972 64
A_Sterner 0:b3cd4463d972 65 DigitalOut SpiReset(p11);
A_Sterner 0:b3cd4463d972 66 DigitalOut IoReset(p12);
A_Sterner 0:b3cd4463d972 67
A_Sterner 2:82e4eac97f0a 68 //PWM
A_Sterner 0:b3cd4463d972 69
A_Sterner 0:b3cd4463d972 70 PwmOut PwmL(p22);
A_Sterner 0:b3cd4463d972 71 PwmOut PwmR(p21);
A_Sterner 0:b3cd4463d972 72
A_Sterner 2:82e4eac97f0a 73 //Serial
A_Sterner 0:b3cd4463d972 74
A_Sterner 0:b3cd4463d972 75 Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable
A_Sterner 6:1d16cc833e0d 76 Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel
A_Sterner 0:b3cd4463d972 77
A_Sterner 2:82e4eac97f0a 78 //SPI
A_Sterner 0:b3cd4463d972 79
A_Sterner 0:b3cd4463d972 80 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
A_Sterner 2:82e4eac97f0a 81
A_Sterner 2:82e4eac97f0a 82 //Interrupts
A_Sterner 2:82e4eac97f0a 83
A_Sterner 0:b3cd4463d972 84 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
A_Sterner 0:b3cd4463d972 85
A_Sterner 0:b3cd4463d972 86
A_Sterner 0:b3cd4463d972 87 // ***************************************************
A_Sterner 0:b3cd4463d972 88 // DE0 Init
A_Sterner 0:b3cd4463d972 89 // ***************************************************
A_Sterner 0:b3cd4463d972 90
A_Sterner 0:b3cd4463d972 91 void DE0_Init(int SpiControlWord){
A_Sterner 0:b3cd4463d972 92
A_Sterner 0:b3cd4463d972 93 int mode = 1;
A_Sterner 0:b3cd4463d972 94 int bits = 16;
A_Sterner 0:b3cd4463d972 95
A_Sterner 0:b3cd4463d972 96 DE0.format(bits,mode);
A_Sterner 0:b3cd4463d972 97
A_Sterner 0:b3cd4463d972 98 // Verify Peripheral ID
A_Sterner 0:b3cd4463d972 99
A_Sterner 0:b3cd4463d972 100 // Generates single square pulse to reset DE0 IO
A_Sterner 0:b3cd4463d972 101
A_Sterner 0:b3cd4463d972 102 IoReset = 0;
A_Sterner 0:b3cd4463d972 103 IoReset = 1;
A_Sterner 0:b3cd4463d972 104 IoReset = 0;
A_Sterner 0:b3cd4463d972 105
A_Sterner 0:b3cd4463d972 106 // Generates single square pulse to reset DE0 SPI
A_Sterner 0:b3cd4463d972 107
A_Sterner 0:b3cd4463d972 108 SpiReset = 0;
A_Sterner 0:b3cd4463d972 109 SpiReset = 1;
A_Sterner 0:b3cd4463d972 110 SpiReset = 0;
A_Sterner 0:b3cd4463d972 111
A_Sterner 0:b3cd4463d972 112 // Writes to DE0 Control Register
A_Sterner 0:b3cd4463d972 113
A_Sterner 0:b3cd4463d972 114 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
A_Sterner 0:b3cd4463d972 115
A_Sterner 0:b3cd4463d972 116 if(ID == 23){ // DE0 ID 23 (0x0017)
A_Sterner 6:1d16cc833e0d 117 Bluetooth.printf("\n\r >> DE0 Initialized.\n\r");}
A_Sterner 0:b3cd4463d972 118 else{
A_Sterner 6:1d16cc833e0d 119 Bluetooth.printf("\n\r >> Failed to initialize DE0 board.\n\r");}
A_Sterner 0:b3cd4463d972 120 }
A_Sterner 0:b3cd4463d972 121
A_Sterner 0:b3cd4463d972 122 // ***************************************************
A_Sterner 0:b3cd4463d972 123 // Left Motor Initialization
A_Sterner 0:b3cd4463d972 124 // ***************************************************
A_Sterner 0:b3cd4463d972 125
A_Sterner 0:b3cd4463d972 126 // Pwm Pin Left Motor : p21
A_Sterner 0:b3cd4463d972 127 // Direction Pin Left Motor : p29
A_Sterner 0:b3cd4463d972 128
A_Sterner 0:b3cd4463d972 129 void L_MotorInit(void){
A_Sterner 0:b3cd4463d972 130
A_Sterner 4:43aef502bb73 131 DirL = 1; // Defaults to 1
A_Sterner 0:b3cd4463d972 132
A_Sterner 0:b3cd4463d972 133 // Direction bit logic output
A_Sterner 0:b3cd4463d972 134 // 0 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 135 // 1 : Forwards ( Advance )
A_Sterner 0:b3cd4463d972 136
A_Sterner 0:b3cd4463d972 137 PwmL.period_us(100);
A_Sterner 4:43aef502bb73 138 PwmL.write(0);
A_Sterner 0:b3cd4463d972 139
A_Sterner 0:b3cd4463d972 140 }
A_Sterner 0:b3cd4463d972 141
A_Sterner 0:b3cd4463d972 142 // ***************************************************
A_Sterner 0:b3cd4463d972 143 // Right Motor Initialization
A_Sterner 0:b3cd4463d972 144 // ***************************************************
A_Sterner 0:b3cd4463d972 145
A_Sterner 0:b3cd4463d972 146 // Pwm Pin Right Motor : p22
A_Sterner 0:b3cd4463d972 147 // Direction Pin Right Motor : p30
A_Sterner 0:b3cd4463d972 148
A_Sterner 0:b3cd4463d972 149 void R_MotorInit(void){
A_Sterner 0:b3cd4463d972 150
A_Sterner 4:43aef502bb73 151 DirR = 0; // Defaults to 0.
A_Sterner 0:b3cd4463d972 152
A_Sterner 0:b3cd4463d972 153 // Direction bit logic output
A_Sterner 2:82e4eac97f0a 154 // 0 : Forwards ( Advance )
A_Sterner 2:82e4eac97f0a 155 // 1 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 156
A_Sterner 0:b3cd4463d972 157 PwmR.period_us(100);
A_Sterner 2:82e4eac97f0a 158 PwmR.write(0);
A_Sterner 0:b3cd4463d972 159
A_Sterner 0:b3cd4463d972 160 }
A_Sterner 0:b3cd4463d972 161
A_Sterner 0:b3cd4463d972 162 /// ***************************************************
A_Sterner 5:e33e69d4eecf 163 // User Input Left
A_Sterner 0:b3cd4463d972 164 // ***************************************************
A_Sterner 0:b3cd4463d972 165
A_Sterner 5:e33e69d4eecf 166 signed int InputLeft(void){
A_Sterner 0:b3cd4463d972 167
A_Sterner 2:82e4eac97f0a 168 signed int input;
A_Sterner 0:b3cd4463d972 169
JordanWisdom 8:9609a50ea076 170 Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (rads/sec) >> ");
JordanWisdom 8:9609a50ea076 171 Bluetooth.scanf("%i",&input);
JordanWisdom 8:9609a50ea076 172
JordanWisdom 8:9609a50ea076 173 Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
JordanWisdom 8:9609a50ea076 174
JordanWisdom 8:9609a50ea076 175 return input;
JordanWisdom 8:9609a50ea076 176
JordanWisdom 8:9609a50ea076 177 }
JordanWisdom 8:9609a50ea076 178
JordanWisdom 8:9609a50ea076 179 signed int InputLeft_m(void){
JordanWisdom 8:9609a50ea076 180
JordanWisdom 8:9609a50ea076 181 signed int input;
JordanWisdom 8:9609a50ea076 182
JordanWisdom 7:241bde733699 183 Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (m/sec) >> ");
A_Sterner 6:1d16cc833e0d 184 Bluetooth.scanf("%i",&input);
A_Sterner 5:e33e69d4eecf 185
A_Sterner 6:1d16cc833e0d 186 Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
A_Sterner 5:e33e69d4eecf 187
A_Sterner 5:e33e69d4eecf 188 return input;
A_Sterner 5:e33e69d4eecf 189
A_Sterner 5:e33e69d4eecf 190 }
A_Sterner 5:e33e69d4eecf 191
A_Sterner 5:e33e69d4eecf 192 /// ***************************************************
A_Sterner 5:e33e69d4eecf 193 // User Input Right
A_Sterner 5:e33e69d4eecf 194 // ***************************************************
A_Sterner 5:e33e69d4eecf 195
A_Sterner 5:e33e69d4eecf 196 signed int InputRight(void){
A_Sterner 5:e33e69d4eecf 197
A_Sterner 5:e33e69d4eecf 198 signed int input;
A_Sterner 5:e33e69d4eecf 199
JordanWisdom 8:9609a50ea076 200 Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (rads/sec) >> ");
JordanWisdom 8:9609a50ea076 201 Bluetooth.scanf("%i",&input);
JordanWisdom 8:9609a50ea076 202
JordanWisdom 8:9609a50ea076 203 Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
JordanWisdom 8:9609a50ea076 204
JordanWisdom 8:9609a50ea076 205 return input;
JordanWisdom 8:9609a50ea076 206
JordanWisdom 8:9609a50ea076 207 }
JordanWisdom 8:9609a50ea076 208
JordanWisdom 8:9609a50ea076 209 signed int InputRight_m(void){
JordanWisdom 8:9609a50ea076 210
JordanWisdom 8:9609a50ea076 211 signed int input;
JordanWisdom 8:9609a50ea076 212
JordanWisdom 7:241bde733699 213 Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (m/sec) >> ");
A_Sterner 6:1d16cc833e0d 214 Bluetooth.scanf("%i",&input);
A_Sterner 5:e33e69d4eecf 215
A_Sterner 6:1d16cc833e0d 216 Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
A_Sterner 0:b3cd4463d972 217
A_Sterner 2:82e4eac97f0a 218 return input;
A_Sterner 0:b3cd4463d972 219
A_Sterner 0:b3cd4463d972 220 }
A_Sterner 0:b3cd4463d972 221
A_Sterner 2:82e4eac97f0a 222 /// ***************************************************
A_Sterner 2:82e4eac97f0a 223 // Control Thread
A_Sterner 2:82e4eac97f0a 224 // ***************************************************
A_Sterner 2:82e4eac97f0a 225
A_Sterner 2:82e4eac97f0a 226 void ControlThread(void){
A_Sterner 2:82e4eac97f0a 227
A_Sterner 2:82e4eac97f0a 228 // Read Incremental Position from DE0 QEI
A_Sterner 0:b3cd4463d972 229
A_Sterner 2:82e4eac97f0a 230 int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register
A_Sterner 2:82e4eac97f0a 231
A_Sterner 2:82e4eac97f0a 232 dPositionLeft = SignExtend(DE0.write(dummy));
A_Sterner 2:82e4eac97f0a 233 dTimeLeft = DE0.write(dummy);
A_Sterner 4:43aef502bb73 234 dPositionRight = SignExtend(DE0.write(dummy));
A_Sterner 4:43aef502bb73 235 dTimeRight = DE0.write(dummy);
A_Sterner 2:82e4eac97f0a 236
A_Sterner 2:82e4eac97f0a 237 // Computer Angular Speed and Angular Speed Error
A_Sterner 2:82e4eac97f0a 238
A_Sterner 2:82e4eac97f0a 239 signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
A_Sterner 4:43aef502bb73 240 signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
A_Sterner 2:82e4eac97f0a 241
A_Sterner 4:43aef502bb73 242 L_e = L_setpoint - AngularSpeedLeft;
A_Sterner 4:43aef502bb73 243 R_e = R_setpoint - AngularSpeedRight;
A_Sterner 4:43aef502bb73 244
A_Sterner 4:43aef502bb73 245 float Kp_L = 2.5;
A_Sterner 4:43aef502bb73 246 float Ki_L = 0.010;
A_Sterner 2:82e4eac97f0a 247
A_Sterner 4:43aef502bb73 248 float Kp_R = 2.5;
A_Sterner 4:43aef502bb73 249 float Ki_R = 0.010;
A_Sterner 2:82e4eac97f0a 250
A_Sterner 4:43aef502bb73 251
A_Sterner 5:e33e69d4eecf 252 if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
A_Sterner 4:43aef502bb73 253 L_integrator = L_integrator +L_e;}
A_Sterner 2:82e4eac97f0a 254 else{
A_Sterner 2:82e4eac97f0a 255 L_integrator = L_integrator;
A_Sterner 0:b3cd4463d972 256 }
A_Sterner 2:82e4eac97f0a 257
A_Sterner 5:e33e69d4eecf 258 if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){
A_Sterner 4:43aef502bb73 259 R_integrator = R_integrator +R_e;}
A_Sterner 4:43aef502bb73 260 else{
A_Sterner 4:43aef502bb73 261 R_integrator = R_integrator;
A_Sterner 4:43aef502bb73 262 }
A_Sterner 5:e33e69d4eecf 263
A_Sterner 4:43aef502bb73 264 L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
A_Sterner 4:43aef502bb73 265 R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
A_Sterner 4:43aef502bb73 266
A_Sterner 5:e33e69d4eecf 267 if(L_u <=0)
A_Sterner 5:e33e69d4eecf 268 DirL = 0;
A_Sterner 5:e33e69d4eecf 269 else
A_Sterner 5:e33e69d4eecf 270 DirL = 1;
A_Sterner 5:e33e69d4eecf 271
A_Sterner 5:e33e69d4eecf 272 if(R_u <=0)
A_Sterner 5:e33e69d4eecf 273 DirR = 1;
A_Sterner 5:e33e69d4eecf 274 else
A_Sterner 5:e33e69d4eecf 275 DirR = 0;
A_Sterner 5:e33e69d4eecf 276
A_Sterner 5:e33e69d4eecf 277 PwmL.write(abs(L_u));
A_Sterner 5:e33e69d4eecf 278 PwmR.write(abs(R_u));
A_Sterner 0:b3cd4463d972 279 }
A_Sterner 0:b3cd4463d972 280
A_Sterner 2:82e4eac97f0a 281 /// ***************************************************
A_Sterner 2:82e4eac97f0a 282 // SaturateAdd
A_Sterner 2:82e4eac97f0a 283 // ***************************************************
A_Sterner 2:82e4eac97f0a 284
A_Sterner 2:82e4eac97f0a 285 signed int SaturateAdd(signed int x, signed int y){
A_Sterner 2:82e4eac97f0a 286
A_Sterner 2:82e4eac97f0a 287 signed int z = x + y;
A_Sterner 2:82e4eac97f0a 288
A_Sterner 2:82e4eac97f0a 289 if( (x>0) && (y>0)&& (z<=0) ){
A_Sterner 2:82e4eac97f0a 290 z = 0x7FFFFFFF;}
A_Sterner 2:82e4eac97f0a 291
A_Sterner 2:82e4eac97f0a 292 else if( (x<0)&&(y<0)&&(z>=0) ){
A_Sterner 2:82e4eac97f0a 293 z = 0x80000000;}
A_Sterner 2:82e4eac97f0a 294
A_Sterner 2:82e4eac97f0a 295 return z;
A_Sterner 2:82e4eac97f0a 296 }
A_Sterner 2:82e4eac97f0a 297
A_Sterner 2:82e4eac97f0a 298 /// ***************************************************
A_Sterner 2:82e4eac97f0a 299 // SaturateLimit
A_Sterner 2:82e4eac97f0a 300 // ***************************************************
A_Sterner 2:82e4eac97f0a 301
A_Sterner 2:82e4eac97f0a 302 float SaturateLimit(float x, float limit){
A_Sterner 2:82e4eac97f0a 303
A_Sterner 2:82e4eac97f0a 304 if (x > limit){
A_Sterner 2:82e4eac97f0a 305 return limit;}
A_Sterner 2:82e4eac97f0a 306
A_Sterner 2:82e4eac97f0a 307 else if(x < -limit){
A_Sterner 2:82e4eac97f0a 308 return(-limit);}
A_Sterner 2:82e4eac97f0a 309
A_Sterner 2:82e4eac97f0a 310 else{
A_Sterner 2:82e4eac97f0a 311 return x;}
A_Sterner 2:82e4eac97f0a 312
A_Sterner 2:82e4eac97f0a 313 }
A_Sterner 2:82e4eac97f0a 314
A_Sterner 2:82e4eac97f0a 315 /// ***************************************************
A_Sterner 2:82e4eac97f0a 316 // Sign Extend
A_Sterner 2:82e4eac97f0a 317 // ***************************************************
A_Sterner 2:82e4eac97f0a 318
A_Sterner 2:82e4eac97f0a 319 signed int SignExtend(int x){
A_Sterner 2:82e4eac97f0a 320
A_Sterner 2:82e4eac97f0a 321 if(x&0x00008000){
A_Sterner 2:82e4eac97f0a 322 x = x|0xFFFF0000;
A_Sterner 2:82e4eac97f0a 323 }
A_Sterner 2:82e4eac97f0a 324
A_Sterner 2:82e4eac97f0a 325 return x;
A_Sterner 2:82e4eac97f0a 326 }
A_Sterner 2:82e4eac97f0a 327
A_Sterner 2:82e4eac97f0a 328
A_Sterner 2:82e4eac97f0a 329
A_Sterner 2:82e4eac97f0a 330 // ==============================================================================================================
A_Sterner 2:82e4eac97f0a 331 // ==============================================================================================================
A_Sterner 2:82e4eac97f0a 332
A_Sterner 2:82e4eac97f0a 333
A_Sterner 2:82e4eac97f0a 334 // *********************************************************************
A_Sterner 2:82e4eac97f0a 335 // MAIN FUNCTION
A_Sterner 2:82e4eac97f0a 336 // *********************************************************************
A_Sterner 2:82e4eac97f0a 337
A_Sterner 2:82e4eac97f0a 338 int main(){
A_Sterner 2:82e4eac97f0a 339
A_Sterner 2:82e4eac97f0a 340 // Initialization
A_Sterner 2:82e4eac97f0a 341
A_Sterner 4:43aef502bb73 342 DE0_Init(0x8004);
A_Sterner 2:82e4eac97f0a 343 L_MotorInit();
A_Sterner 4:43aef502bb73 344 R_MotorInit();
A_Sterner 2:82e4eac97f0a 345 L_integrator = 0;
A_Sterner 4:43aef502bb73 346 R_integrator = 0;
A_Sterner 2:82e4eac97f0a 347 ControlInterrupt.attach(&ControlThread, 0.0005);
A_Sterner 2:82e4eac97f0a 348
A_Sterner 2:82e4eac97f0a 349 // Specify Setpoint ( rads/sec )
JordanWisdom 8:9609a50ea076 350 //
JordanWisdom 8:9609a50ea076 351 L_setpoint = InputLeft();
JordanWisdom 8:9609a50ea076 352 R_setpoint = InputRight();
JordanWisdom 8:9609a50ea076 353
JordanWisdom 8:9609a50ea076 354 // Specify Setpoint ( m/sec )
JordanWisdom 8:9609a50ea076 355 /*
JordanWisdom 8:9609a50ea076 356 L_setpoint = InputLeft_m();
JordanWisdom 8:9609a50ea076 357 R_setpoint = InputRight_m();*/
JordanWisdom 8:9609a50ea076 358
A_Sterner 2:82e4eac97f0a 359 // Display Global Variables to Console
A_Sterner 6:1d16cc833e0d 360 Bluetooth.printf("\n\r ========= LEFT ======= ========= RIGHT =======");
JordanWisdom 7:241bde733699 361 Bluetooth.printf("\n\r US VE IS CS US VE IS CS ");
A_Sterner 2:82e4eac97f0a 362
A_Sterner 2:82e4eac97f0a 363 while(1){
A_Sterner 2:82e4eac97f0a 364
A_Sterner 4:43aef502bb73 365 float L_error_t = L_e;
JordanWisdom 8:9609a50ea076 366 float L_error_m = L_e*0.05093; //Multiply by 0.05093 for m/s
JordanWisdom 8:9609a50ea076 367 float L_u_t = L_u;
A_Sterner 4:43aef502bb73 368
JordanWisdom 8:9609a50ea076 369 float R_error_t = R_e; //Multiply by 0.05093 for m/s
A_Sterner 4:43aef502bb73 370 float R_u_t = R_u;
JordanWisdom 8:9609a50ea076 371 float R_error_m = R_e*0.05093; //Multiply by 0.05093 for m/s
A_Sterner 2:82e4eac97f0a 372
JordanWisdom 8:9609a50ea076 373 float L_setpoint = L_setpoint; //Setpoints in float values for display
JordanWisdom 8:9609a50ea076 374 float R_setpoint = R_setpoint; //Setpoints in float values for display
JordanWisdom 8:9609a50ea076 375
JordanWisdom 8:9609a50ea076 376 float L_setpoint_m = L_setpoint*0.05093; //Setpoints in float values for display
JordanWisdom 8:9609a50ea076 377 float R_setpoint_m = R_setpoint*0.05093; //Setpoints in float values for display
A_Sterner 6:1d16cc833e0d 378
JordanWisdom 8:9609a50ea076 379 // M/S Output
JordanWisdom 8:9609a50ea076 380 //Bluetooth.printf("\n\r %2.2f %2.1f %i %2.2f %2.2f %2.1f %i %2.2f",L_setpoint_m,L_error_m,L_integrator,L_u_t,R_setpoint_m,R_error_m,R_integrator,R_u_t);
JordanWisdom 8:9609a50ea076 381 // Rad/S Output
JordanWisdom 8:9609a50ea076 382 Bluetooth.printf("\n\r %2.2f %2.1f %i %2.2f %2.2f %2.1f %i %2.2f",L_setpoint,L_error_t,L_integrator,L_u_t,R_setpoint,R_error_t,R_integrator,R_u_t);
A_Sterner 2:82e4eac97f0a 383 wait(0.75);
A_Sterner 2:82e4eac97f0a 384 }
A_Sterner 2:82e4eac97f0a 385
A_Sterner 2:82e4eac97f0a 386 }