ECE 4333 / Mbed 2 deprecated Motion_Primitive

motion primitives

Dependencies:   mbed

Fork of Motion_Primitive_Test by ECE 4333

MotionTest.cpp@2:0f93881225d2, 2016-03-18 (annotated)

Committer:
JordanWisdom
Date:
Fri Mar 18 16:08:50 2016 +0000
Revision:
2:0f93881225d2
Parent:
1:d523415c7b53 
updated

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