ECE 4333 / Mbed 2 deprecated Motion_Primitive_Test

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

MotionTest.cpp@4:1bcb35cef400, 2016-03-19 (annotated)

Committer:
JordanWisdom
Date:
Sat Mar 19 19:32:53 2016 +0000
Revision:
4:1bcb35cef400
Parent:
3:9cbf571137a5
Child:
5:ac5819613d0c
- Changed controller to float values; - Changed outputs to floats and removed redundant variables; - Tidied the code (ongoing)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JordanWisdom 0:4c63f2192998 1 // EE4333 Robotics Lab 3
JordanWisdom 0:4c63f2192998 2
JordanWisdom 0:4c63f2192998 3 // Library Imports
JordanWisdom 0:4c63f2192998 4
JordanWisdom 0:4c63f2192998 5 //#include "InterruptIn.h"
JordanWisdom 0:4c63f2192998 6 //#include "rtos.h"
JordanWisdom 0:4c63f2192998 7 #include "mbed.h"
JordanWisdom 0:4c63f2192998 8 #include "Serial.h"
JordanWisdom 0:4c63f2192998 9 #include "stdio.h"
JordanWisdom 0:4c63f2192998 10
JordanWisdom 0:4c63f2192998 11 //Function Declarations
JordanWisdom 0:4c63f2192998 12
JordanWisdom 0:4c63f2192998 13 void DE0_Init(int);
JordanWisdom 0:4c63f2192998 14 void L_MotorInit(void);
JordanWisdom 0:4c63f2192998 15 void R_MotorInit(void);
JordanWisdom 0:4c63f2192998 16 signed int InputLeft(void);
JordanWisdom 0:4c63f2192998 17 signed int InputRight(void);
JordanWisdom 0:4c63f2192998 18 void ControlThread(void);
JordanWisdom 0:4c63f2192998 19 int SaturateAdd(int x, int y);
JordanWisdom 0:4c63f2192998 20 float SaturateLimit(float x, float limit);
JordanWisdom 0:4c63f2192998 21 signed int SignExtend(signed int x);
JordanWisdom 1:d523415c7b53 22 char GUI();
JordanWisdom 1:d523415c7b53 23 void Settings(char x);
JordanWisdom 0:4c63f2192998 24
JordanWisdom 0:4c63f2192998 25 // ********************************************************************
JordanWisdom 0:4c63f2192998 26 // GLOBAL VARIABLE DECLARATIONS
JordanWisdom 0:4c63f2192998 27 // ********************************************************************
JordanWisdom 0:4c63f2192998 28
JordanWisdom 0:4c63f2192998 29 signed int R_setpoint; // Desired Angular Speed ( rad/sec )
JordanWisdom 0:4c63f2192998 30 signed int L_setpoint;
JordanWisdom 0:4c63f2192998 31
JordanWisdom 0:4c63f2192998 32 signed int R_setpoint_m; // Desired Angular Speed ( m/sec )
JordanWisdom 0:4c63f2192998 33 signed int L_setpoint_m;
JordanWisdom 0:4c63f2192998 34
JordanWisdom 0:4c63f2192998 35 float R_e; // Velocity Error
JordanWisdom 0:4c63f2192998 36 float R_u; // Control Signal
JordanWisdom 0:4c63f2192998 37 float L_e;
JordanWisdom 0:4c63f2192998 38 float L_u;
JordanWisdom 0:4c63f2192998 39 int L_integrator; // Left Integrator State
JordanWisdom 0:4c63f2192998 40 int R_integrator;
JordanWisdom 0:4c63f2192998 41 signed int dPositionLeft; // DE0 Register 0
JordanWisdom 0:4c63f2192998 42 signed int dPositionRight;
JordanWisdom 0:4c63f2192998 43 int dTimeLeft; // DE0 Register 1
JordanWisdom 0:4c63f2192998 44 int dTimeRight;
JordanWisdom 1:d523415c7b53 45 float Circ = 0;
JordanWisdom 1:d523415c7b53 46 float tTot = 0;
JordanWisdom 4:1bcb35cef400 47 float AngularSpeedLeft;
JordanWisdom 4:1bcb35cef400 48 float AngularSpeedRight;
JordanWisdom 4:1bcb35cef400 49 //signed int AngularSpeedLeft;
JordanWisdom 4:1bcb35cef400 50 //signed int AngularSpeedRight;
JordanWisdom 0:4c63f2192998 51
JordanWisdom 0:4c63f2192998 52 //Motion Primitive Variables
JordanWisdom 0:4c63f2192998 53 signed int Arc;
JordanWisdom 1:d523415c7b53 54 float Radius = 0;
JordanWisdom 1:d523415c7b53 55 float VelDes = 0;
JordanWisdom 1:d523415c7b53 56 float tcalc = 0;
JordanWisdom 1:d523415c7b53 57
JordanWisdom 0:4c63f2192998 58 float O_setpoint; //Inner Wheel Speed
JordanWisdom 0:4c63f2192998 59 float I_setpoint; //Outer Wheel Speed
JordanWisdom 1:d523415c7b53 60 float L = 5.5*2.54/100; //Distance between wheel contacts ; //Distance between wheels (in meters)
JordanWisdom 0:4c63f2192998 61 float pi = 3.141259;
JordanWisdom 0:4c63f2192998 62 // *********************************************************************
JordanWisdom 0:4c63f2192998 63 // PROCESSES AND THREADS
JordanWisdom 0:4c63f2192998 64 // *********************************************************************
JordanWisdom 0:4c63f2192998 65
JordanWisdom 0:4c63f2192998 66
JordanWisdom 0:4c63f2192998 67 // *********************************************************************
JordanWisdom 0:4c63f2192998 68 // PIN DECLARATIONS
JordanWisdom 0:4c63f2192998 69 // *********************************************************************
JordanWisdom 0:4c63f2192998 70
JordanWisdom 0:4c63f2192998 71 // Digital I/O Pins
JordanWisdom 0:4c63f2192998 72
JordanWisdom 0:4c63f2192998 73 DigitalOut led1(LED1); // Thread Indicators
JordanWisdom 0:4c63f2192998 74 DigitalOut led2(LED2); //
JordanWisdom 0:4c63f2192998 75 DigitalOut led3(LED3); //
JordanWisdom 0:4c63f2192998 76 DigitalOut led4(LED4); //
JordanWisdom 0:4c63f2192998 77
JordanWisdom 0:4c63f2192998 78 DigitalOut DirL(p29); // Direction of Left Motor
JordanWisdom 0:4c63f2192998 79 DigitalOut DirR(p30); // Direction of Right Motor
JordanWisdom 0:4c63f2192998 80
JordanWisdom 0:4c63f2192998 81 // SPI Related Digital I/O Pins
JordanWisdom 0:4c63f2192998 82
JordanWisdom 0:4c63f2192998 83 DigitalOut SpiReset(p11);
JordanWisdom 0:4c63f2192998 84 DigitalOut IoReset(p12);
JordanWisdom 0:4c63f2192998 85
JordanWisdom 0:4c63f2192998 86 //PWM
JordanWisdom 0:4c63f2192998 87
JordanWisdom 0:4c63f2192998 88 PwmOut PwmL(p22);
JordanWisdom 0:4c63f2192998 89 PwmOut PwmR(p21);
JordanWisdom 0:4c63f2192998 90
JordanWisdom 0:4c63f2192998 91 //Serial
JordanWisdom 0:4c63f2192998 92
JordanWisdom 0:4c63f2192998 93 Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable
JordanWisdom 0:4c63f2192998 94 Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel
JordanWisdom 0:4c63f2192998 95
JordanWisdom 0:4c63f2192998 96 //SPI
JordanWisdom 0:4c63f2192998 97
JordanWisdom 0:4c63f2192998 98 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
JordanWisdom 0:4c63f2192998 99
JordanWisdom 0:4c63f2192998 100 //Interrupts
JordanWisdom 0:4c63f2192998 101
JordanWisdom 0:4c63f2192998 102 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
JordanWisdom 0:4c63f2192998 103
JordanWisdom 0:4c63f2192998 104
JordanWisdom 0:4c63f2192998 105 // ***************************************************
JordanWisdom 0:4c63f2192998 106 // DE0 Init
JordanWisdom 0:4c63f2192998 107 // ***************************************************
JordanWisdom 0:4c63f2192998 108
JordanWisdom 0:4c63f2192998 109 void DE0_Init(int SpiControlWord){
JordanWisdom 0:4c63f2192998 110
JordanWisdom 0:4c63f2192998 111 int mode = 1;
JordanWisdom 0:4c63f2192998 112 int bits = 16;
JordanWisdom 0:4c63f2192998 113
JordanWisdom 0:4c63f2192998 114 DE0.format(bits,mode);
JordanWisdom 0:4c63f2192998 115
JordanWisdom 0:4c63f2192998 116 // Verify Peripheral ID
JordanWisdom 0:4c63f2192998 117
JordanWisdom 0:4c63f2192998 118 // Generates single square pulse to reset DE0 IO
JordanWisdom 0:4c63f2192998 119
JordanWisdom 0:4c63f2192998 120 IoReset = 0;
JordanWisdom 0:4c63f2192998 121 IoReset = 1;
JordanWisdom 0:4c63f2192998 122 IoReset = 0;
JordanWisdom 0:4c63f2192998 123
JordanWisdom 0:4c63f2192998 124 // Generates single square pulse to reset DE0 SPI
JordanWisdom 0:4c63f2192998 125
JordanWisdom 0:4c63f2192998 126 SpiReset = 0;
JordanWisdom 0:4c63f2192998 127 SpiReset = 1;
JordanWisdom 0:4c63f2192998 128 SpiReset = 0;
JordanWisdom 0:4c63f2192998 129
JordanWisdom 0:4c63f2192998 130 // Writes to DE0 Control Register
JordanWisdom 0:4c63f2192998 131
JordanWisdom 0:4c63f2192998 132 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
JordanWisdom 0:4c63f2192998 133
JordanWisdom 0:4c63f2192998 134 if(ID == 23){ // DE0 ID 23 (0x0017)
JordanWisdom 0:4c63f2192998 135 Bluetooth.printf("\n\r >> DE0 Initialized.\n\r");}
JordanWisdom 0:4c63f2192998 136 else{
JordanWisdom 0:4c63f2192998 137 Bluetooth.printf("\n\r >> Failed to initialize DE0 board.\n\r");}
JordanWisdom 0:4c63f2192998 138 }
JordanWisdom 0:4c63f2192998 139
JordanWisdom 0:4c63f2192998 140 // ***************************************************
JordanWisdom 0:4c63f2192998 141 // Left Motor Initialization
JordanWisdom 0:4c63f2192998 142 // ***************************************************
JordanWisdom 0:4c63f2192998 143
JordanWisdom 0:4c63f2192998 144 // Pwm Pin Left Motor : p21
JordanWisdom 0:4c63f2192998 145 // Direction Pin Left Motor : p29
JordanWisdom 0:4c63f2192998 146
JordanWisdom 0:4c63f2192998 147 void L_MotorInit(void){
JordanWisdom 0:4c63f2192998 148
JordanWisdom 0:4c63f2192998 149 DirL = 1; // Defaults to 1
JordanWisdom 0:4c63f2192998 150
JordanWisdom 0:4c63f2192998 151 // Direction bit logic output
JordanWisdom 0:4c63f2192998 152 // 0 : Backwards ( Reverse )
JordanWisdom 0:4c63f2192998 153 // 1 : Forwards ( Advance )
JordanWisdom 0:4c63f2192998 154
JordanWisdom 0:4c63f2192998 155 PwmL.period_us(100);
JordanWisdom 0:4c63f2192998 156 PwmL.write(0);
JordanWisdom 0:4c63f2192998 157
JordanWisdom 0:4c63f2192998 158 }
JordanWisdom 0:4c63f2192998 159
JordanWisdom 0:4c63f2192998 160 // ***************************************************
JordanWisdom 0:4c63f2192998 161 // Right Motor Initialization
JordanWisdom 0:4c63f2192998 162 // ***************************************************
JordanWisdom 0:4c63f2192998 163
JordanWisdom 0:4c63f2192998 164 // Pwm Pin Right Motor : p22
JordanWisdom 0:4c63f2192998 165 // Direction Pin Right Motor : p30
JordanWisdom 0:4c63f2192998 166
JordanWisdom 0:4c63f2192998 167 void R_MotorInit(void){
JordanWisdom 0:4c63f2192998 168
JordanWisdom 0:4c63f2192998 169 DirR = 0; // Defaults to 0.
JordanWisdom 0:4c63f2192998 170
JordanWisdom 0:4c63f2192998 171 // Direction bit logic output
JordanWisdom 0:4c63f2192998 172 // 0 : Forwards ( Advance )
JordanWisdom 0:4c63f2192998 173 // 1 : Backwards ( Reverse )
JordanWisdom 0:4c63f2192998 174
JordanWisdom 0:4c63f2192998 175 PwmR.period_us(100);
JordanWisdom 0:4c63f2192998 176 PwmR.write(0);
JordanWisdom 0:4c63f2192998 177
JordanWisdom 0:4c63f2192998 178 }
JordanWisdom 0:4c63f2192998 179
JordanWisdom 0:4c63f2192998 180
JordanWisdom 0:4c63f2192998 181 /// ***************************************************
JordanWisdom 0:4c63f2192998 182 // Control Thread
JordanWisdom 0:4c63f2192998 183 // ***************************************************
JordanWisdom 0:4c63f2192998 184
JordanWisdom 0:4c63f2192998 185 void ControlThread(void){
JordanWisdom 0:4c63f2192998 186
JordanWisdom 0:4c63f2192998 187 // Read Incremental Position from DE0 QEI
JordanWisdom 0:4c63f2192998 188 int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register
JordanWisdom 0:4c63f2192998 189
JordanWisdom 0:4c63f2192998 190 dPositionLeft = SignExtend(DE0.write(dummy));
JordanWisdom 0:4c63f2192998 191 dTimeLeft = DE0.write(dummy);
JordanWisdom 0:4c63f2192998 192 dPositionRight = SignExtend(DE0.write(dummy));
JordanWisdom 0:4c63f2192998 193 dTimeRight = DE0.write(dummy);
JordanWisdom 0:4c63f2192998 194
JordanWisdom 0:4c63f2192998 195 // Computer Angular Speed and Angular Speed Error
JordanWisdom 0:4c63f2192998 196
JordanWisdom 3:9cbf571137a5 197 AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
JordanWisdom 3:9cbf571137a5 198 AngularSpeedRight = (123*dPositionRight)/dTimeRight;
JordanWisdom 0:4c63f2192998 199
JordanWisdom 0:4c63f2192998 200 //CHANGED FOR OUTER AND INNER WHEELS
JordanWisdom 0:4c63f2192998 201 L_e = O_setpoint - AngularSpeedLeft;
JordanWisdom 0:4c63f2192998 202 R_e = I_setpoint - AngularSpeedRight;
JordanWisdom 0:4c63f2192998 203
JordanWisdom 0:4c63f2192998 204 float Kp_L = 2.5;
JordanWisdom 0:4c63f2192998 205 float Ki_L = 0.010;
JordanWisdom 0:4c63f2192998 206
JordanWisdom 0:4c63f2192998 207 float Kp_R = 2.5;
JordanWisdom 0:4c63f2192998 208 float Ki_R = 0.010;
JordanWisdom 4:1bcb35cef400 209
JordanWisdom 0:4c63f2192998 210 // Saturate Inegrators if necessary
JordanWisdom 0:4c63f2192998 211 if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
JordanWisdom 0:4c63f2192998 212 L_integrator = L_integrator +L_e;}
JordanWisdom 0:4c63f2192998 213 else{
JordanWisdom 0:4c63f2192998 214 L_integrator = L_integrator;
JordanWisdom 0:4c63f2192998 215 }
JordanWisdom 0:4c63f2192998 216
JordanWisdom 0:4c63f2192998 217 if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){
JordanWisdom 0:4c63f2192998 218 R_integrator = R_integrator +R_e;}
JordanWisdom 0:4c63f2192998 219 else{
JordanWisdom 0:4c63f2192998 220 R_integrator = R_integrator;
JordanWisdom 0:4c63f2192998 221 }
JordanWisdom 0:4c63f2192998 222
JordanWisdom 0:4c63f2192998 223 L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
JordanWisdom 0:4c63f2192998 224 R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
JordanWisdom 0:4c63f2192998 225
JordanWisdom 0:4c63f2192998 226 if(L_u <=0)
JordanWisdom 0:4c63f2192998 227 DirL = 0;
JordanWisdom 0:4c63f2192998 228 else
JordanWisdom 0:4c63f2192998 229 DirL = 1;
JordanWisdom 0:4c63f2192998 230
JordanWisdom 0:4c63f2192998 231 if(R_u <=0)
JordanWisdom 0:4c63f2192998 232 DirR = 1;
JordanWisdom 0:4c63f2192998 233 else
JordanWisdom 0:4c63f2192998 234 DirR = 0;
JordanWisdom 0:4c63f2192998 235
JordanWisdom 0:4c63f2192998 236 PwmL.write(abs(L_u));
JordanWisdom 0:4c63f2192998 237 PwmR.write(abs(R_u));
JordanWisdom 0:4c63f2192998 238 }
JordanWisdom 0:4c63f2192998 239
JordanWisdom 4:1bcb35cef400 240 /*// *************************************************** NOT USED
JordanWisdom 0:4c63f2192998 241 // SaturateAdd
JordanWisdom 0:4c63f2192998 242 // ***************************************************
JordanWisdom 0:4c63f2192998 243
JordanWisdom 0:4c63f2192998 244 signed int SaturateAdd(signed int x, signed int y){
JordanWisdom 0:4c63f2192998 245
JordanWisdom 0:4c63f2192998 246 signed int z = x + y;
JordanWisdom 0:4c63f2192998 247
JordanWisdom 0:4c63f2192998 248 if( (x>0) && (y>0)&& (z<=0) ){
JordanWisdom 0:4c63f2192998 249 z = 0x7FFFFFFF;}
JordanWisdom 0:4c63f2192998 250
JordanWisdom 0:4c63f2192998 251 else if( (x<0)&&(y<0)&&(z>=0) ){
JordanWisdom 0:4c63f2192998 252 z = 0x80000000;}
JordanWisdom 0:4c63f2192998 253
JordanWisdom 0:4c63f2192998 254 return z;
JordanWisdom 4:1bcb35cef400 255 }*/
JordanWisdom 0:4c63f2192998 256
JordanWisdom 0:4c63f2192998 257 /// ***************************************************
JordanWisdom 0:4c63f2192998 258 // SaturateLimit
JordanWisdom 0:4c63f2192998 259 // ***************************************************
JordanWisdom 0:4c63f2192998 260
JordanWisdom 0:4c63f2192998 261 float SaturateLimit(float x, float limit){
JordanWisdom 0:4c63f2192998 262
JordanWisdom 0:4c63f2192998 263 if (x > limit){
JordanWisdom 0:4c63f2192998 264 return limit;
JordanWisdom 0:4c63f2192998 265 }
JordanWisdom 0:4c63f2192998 266 else if(x < -limit){
JordanWisdom 0:4c63f2192998 267 return(-limit);
JordanWisdom 0:4c63f2192998 268 }
JordanWisdom 0:4c63f2192998 269 else{
JordanWisdom 0:4c63f2192998 270 return x;}
JordanWisdom 0:4c63f2192998 271
JordanWisdom 0:4c63f2192998 272 }
JordanWisdom 0:4c63f2192998 273
JordanWisdom 0:4c63f2192998 274 /// ***************************************************
JordanWisdom 0:4c63f2192998 275 // Sign Extend
JordanWisdom 0:4c63f2192998 276 // ***************************************************
JordanWisdom 0:4c63f2192998 277
JordanWisdom 0:4c63f2192998 278 signed int SignExtend(int x){
JordanWisdom 0:4c63f2192998 279
JordanWisdom 0:4c63f2192998 280 if(x&0x00008000){
JordanWisdom 0:4c63f2192998 281 x = x|0xFFFF0000;
JordanWisdom 0:4c63f2192998 282 }
JordanWisdom 0:4c63f2192998 283
JordanWisdom 0:4c63f2192998 284 return x;
JordanWisdom 0:4c63f2192998 285 }
JordanWisdom 0:4c63f2192998 286
JordanWisdom 4:1bcb35cef400 287 // ***************************************************
JordanWisdom 4:1bcb35cef400 288 // Startup GUI
JordanWisdom 4:1bcb35cef400 289 // ***************************************************
JordanWisdom 4:1bcb35cef400 290
JordanWisdom 1:d523415c7b53 291 char GUI(){
JordanWisdom 1:d523415c7b53 292 char x;
JordanWisdom 3:9cbf571137a5 293 Bluetooth.printf("\n\r (C)ircle or (L)ine? >>");
JordanWisdom 3:9cbf571137a5 294 Bluetooth.scanf("%c", &x);
JordanWisdom 1:d523415c7b53 295 return x;
JordanWisdom 1:d523415c7b53 296 }
JordanWisdom 0:4c63f2192998 297
JordanWisdom 4:1bcb35cef400 298 // ***************************************************
JordanWisdom 4:1bcb35cef400 299 // Setpoint Collection
JordanWisdom 4:1bcb35cef400 300 // ***************************************************
JordanWisdom 4:1bcb35cef400 301
JordanWisdom 4:1bcb35cef400 302
JordanWisdom 1:d523415c7b53 303 void Settings(char x){
JordanWisdom 1:d523415c7b53 304 char z;
JordanWisdom 1:d523415c7b53 305 if(x == 'c' || x == 'C')
JordanWisdom 1:d523415c7b53 306 {
JordanWisdom 3:9cbf571137a5 307 Bluetooth.printf("\n\rCircle Radius? (m)");
JordanWisdom 3:9cbf571137a5 308 Bluetooth.scanf("%f", &Radius);
JordanWisdom 3:9cbf571137a5 309 Bluetooth.printf("\n\rDesired Speed? (m)");
JordanWisdom 3:9cbf571137a5 310 Bluetooth.scanf("%f", &VelDes);
JordanWisdom 3:9cbf571137a5 311 Bluetooth.printf("\n\r Desired Direction? (L) or (R)");
JordanWisdom 3:9cbf571137a5 312 Bluetooth.scanf("%c",&z);
JordanWisdom 1:d523415c7b53 313 float Circ = 2*pi*Radius;
JordanWisdom 1:d523415c7b53 314 float tTot = Circ/VelDes;
JordanWisdom 1:d523415c7b53 315 if(z == 'l' || z == 'L')
JordanWisdom 1:d523415c7b53 316 {
JordanWisdom 1:d523415c7b53 317 I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
JordanWisdom 1:d523415c7b53 318 if(I_setpoint>35)
JordanWisdom 1:d523415c7b53 319 {
JordanWisdom 1:d523415c7b53 320 I_setpoint = 35;
JordanWisdom 3:9cbf571137a5 321 Bluetooth.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
JordanWisdom 1:d523415c7b53 322 }
JordanWisdom 1:d523415c7b53 323 O_setpoint = I_setpoint*(Radius-L/2)/(Radius+L/2);
JordanWisdom 1:d523415c7b53 324 }
JordanWisdom 1:d523415c7b53 325 else
JordanWisdom 1:d523415c7b53 326 {
JordanWisdom 1:d523415c7b53 327 O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
JordanWisdom 1:d523415c7b53 328 if(O_setpoint>35)
JordanWisdom 1:d523415c7b53 329 {
JordanWisdom 1:d523415c7b53 330 O_setpoint = 35;
JordanWisdom 3:9cbf571137a5 331 Bluetooth.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
JordanWisdom 1:d523415c7b53 332
JordanWisdom 1:d523415c7b53 333 }
JordanWisdom 1:d523415c7b53 334 I_setpoint = (Radius-L/2)*2*pi/((2*2.54/100)*tTot);
JordanWisdom 1:d523415c7b53 335 }
JordanWisdom 1:d523415c7b53 336 }
JordanWisdom 1:d523415c7b53 337 else
JordanWisdom 1:d523415c7b53 338 {
JordanWisdom 3:9cbf571137a5 339 Bluetooth.printf("\n\r Desired Speed? (m) >>");
JordanWisdom 3:9cbf571137a5 340 Bluetooth.scanf("%f", &VelDes);
JordanWisdom 1:d523415c7b53 341 O_setpoint = VelDes/0.05094;
JordanWisdom 1:d523415c7b53 342 if(O_setpoint>35)
JordanWisdom 1:d523415c7b53 343 {
JordanWisdom 1:d523415c7b53 344 O_setpoint = 35;
JordanWisdom 3:9cbf571137a5 345 Bluetooth.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
JordanWisdom 1:d523415c7b53 346 }
JordanWisdom 1:d523415c7b53 347 I_setpoint = O_setpoint;
JordanWisdom 1:d523415c7b53 348 }
JordanWisdom 1:d523415c7b53 349
JordanWisdom 1:d523415c7b53 350
JordanWisdom 1:d523415c7b53 351 return;
JordanWisdom 4:1bcb35cef400 352 }
JordanWisdom 0:4c63f2192998 353
JordanWisdom 0:4c63f2192998 354
JordanWisdom 0:4c63f2192998 355 // *********************************************************************
JordanWisdom 0:4c63f2192998 356 // MAIN FUNCTION
JordanWisdom 0:4c63f2192998 357 // *********************************************************************
JordanWisdom 0:4c63f2192998 358
JordanWisdom 0:4c63f2192998 359 int main(){
JordanWisdom 0:4c63f2192998 360
JordanWisdom 1:d523415c7b53 361 // Initialization
JordanWisdom 0:4c63f2192998 362
JordanWisdom 0:4c63f2192998 363 DE0_Init(0x8004);
JordanWisdom 0:4c63f2192998 364 L_MotorInit();
JordanWisdom 0:4c63f2192998 365 R_MotorInit();
JordanWisdom 0:4c63f2192998 366 L_integrator = 0;
JordanWisdom 0:4c63f2192998 367 R_integrator = 0;
JordanWisdom 0:4c63f2192998 368 ControlInterrupt.attach(&ControlThread, 0.0005);
JordanWisdom 0:4c63f2192998 369
JordanWisdom 0:4c63f2192998 370 //Motion Primitives Initialization
JordanWisdom 4:1bcb35cef400 371 Arc = 0; // Arc Length (start at 0)
JordanWisdom 4:1bcb35cef400 372 Settings(GUI()); // Ask user for input and pass to Settings
JordanWisdom 4:1bcb35cef400 373 float WAvg,VAvg;
JordanWisdom 0:4c63f2192998 374 // Display Global Variables to Console
JordanWisdom 0:4c63f2192998 375 while(1){
JordanWisdom 4:1bcb35cef400 376 WAvg = (AngularSpeedLeft + AngularSpeedRight);
JordanWisdom 4:1bcb35cef400 377 VAvg = (AngularSpeedLeft + AngularSpeedRight)*0.02483*2;
JordanWisdom 4:1bcb35cef400 378 Bluetooth.printf("\n\r||SetLeft: %2.3f ||WLeft: %2.3f rad/s ||SetRight: %2.3f ||WRight: %2.3f rad/s||WAverage: %2.3f||VAverage: %2.3f",O_setpoint,AngularSpeedLeft,I_setpoint,AngularSpeedRight,WAvg,VAvg);
JordanWisdom 0:4c63f2192998 379 wait(0.75);
JordanWisdom 1:d523415c7b53 380 }
JordanWisdom 0:4c63f2192998 381 }