ECE 4333 / Mbed 2 deprecated Motion_Primitive

motion primitives

Dependencies:   mbed

Fork of Motion_Primitive_Test by ECE 4333

MotionTest.cpp@1:d523415c7b53, 2016-03-18 (annotated)

Committer:
JordanWisdom
Date:
Fri Mar 18 14:36:50 2016 +0000
Revision:
1:d523415c7b53
Parent:
0:4c63f2192998
Child:
2:0f93881225d2
Child:
3:9cbf571137a5
Revised and working for March 18th Demo

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 0:4c63f2192998 172 PwmR.write(0);
JordanWisdom 0:4c63f2192998 173
JordanWisdom 0:4c63f2192998 174 }
JordanWisdom 0:4c63f2192998 175
JordanWisdom 0:4c63f2192998 176
JordanWisdom 0:4c63f2192998 177 /// ***************************************************
JordanWisdom 0:4c63f2192998 178 // Control Thread
JordanWisdom 0:4c63f2192998 179 // ***************************************************
JordanWisdom 0:4c63f2192998 180
JordanWisdom 0:4c63f2192998 181 void ControlThread(void){
JordanWisdom 0:4c63f2192998 182
JordanWisdom 0:4c63f2192998 183 // Read Incremental Position from DE0 QEI
JordanWisdom 0:4c63f2192998 184 int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register
JordanWisdom 0:4c63f2192998 185
JordanWisdom 0:4c63f2192998 186 dPositionLeft = SignExtend(DE0.write(dummy));
JordanWisdom 0:4c63f2192998 187 dTimeLeft = DE0.write(dummy);
JordanWisdom 0:4c63f2192998 188 dPositionRight = SignExtend(DE0.write(dummy));
JordanWisdom 0:4c63f2192998 189 dTimeRight = DE0.write(dummy);
JordanWisdom 0:4c63f2192998 190
JordanWisdom 0:4c63f2192998 191 // Computer Angular Speed and Angular Speed Error
JordanWisdom 0:4c63f2192998 192
JordanWisdom 0:4c63f2192998 193 signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
JordanWisdom 0:4c63f2192998 194 signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
JordanWisdom 0:4c63f2192998 195
JordanWisdom 0:4c63f2192998 196 //CHANGED FOR OUTER AND INNER WHEELS
JordanWisdom 0:4c63f2192998 197 L_e = O_setpoint - AngularSpeedLeft;
JordanWisdom 0:4c63f2192998 198 R_e = I_setpoint - AngularSpeedRight;
JordanWisdom 0:4c63f2192998 199
JordanWisdom 0:4c63f2192998 200 float Kp_L = 2.5;
JordanWisdom 0:4c63f2192998 201 float Ki_L = 0.010;
JordanWisdom 0:4c63f2192998 202
JordanWisdom 0:4c63f2192998 203 float Kp_R = 2.5;
JordanWisdom 0:4c63f2192998 204 float Ki_R = 0.010;
JordanWisdom 0:4c63f2192998 205
JordanWisdom 0:4c63f2192998 206 // **** MODIFICATIONS FOR MOTION PRIMITIVE ARE HERE ****
JordanWisdom 0:4c63f2192998 207 // In order to determine theta relative to any given position, we can just use Theta = Arc Length/Radius
JordanWisdom 0:4c63f2192998 208 // Radius R is determined by the user
JordanWisdom 0:4c63f2192998 209 // Arc Length is determined from an integration of average velocity dP/dt, which is tracked by the DE0 board
JordanWisdom 0:4c63f2192998 210
JordanWisdom 0:4c63f2192998 211 //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 212 //Arc = (AngularSpeedLeft*dTimeLeft+AngularSpeedRight*dTimeRight)*0.05093/2; //constant of 0.05093 derived in another function using wheel radii
JordanWisdom 0:4c63f2192998 213
JordanWisdom 0:4c63f2192998 214 // Saturate Inegrators if necessary
JordanWisdom 0:4c63f2192998 215 if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
JordanWisdom 0:4c63f2192998 216 L_integrator = L_integrator +L_e;}
JordanWisdom 0:4c63f2192998 217 else{
JordanWisdom 0:4c63f2192998 218 L_integrator = L_integrator;
JordanWisdom 0:4c63f2192998 219 }
JordanWisdom 0:4c63f2192998 220
JordanWisdom 0:4c63f2192998 221 if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){
JordanWisdom 0:4c63f2192998 222 R_integrator = R_integrator +R_e;}
JordanWisdom 0:4c63f2192998 223 else{
JordanWisdom 0:4c63f2192998 224 R_integrator = R_integrator;
JordanWisdom 0:4c63f2192998 225 }
JordanWisdom 0:4c63f2192998 226
JordanWisdom 0:4c63f2192998 227 L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
JordanWisdom 0:4c63f2192998 228 R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
JordanWisdom 0:4c63f2192998 229
JordanWisdom 0:4c63f2192998 230 if(L_u <=0)
JordanWisdom 0:4c63f2192998 231 DirL = 0;
JordanWisdom 0:4c63f2192998 232 else
JordanWisdom 0:4c63f2192998 233 DirL = 1;
JordanWisdom 0:4c63f2192998 234
JordanWisdom 0:4c63f2192998 235 if(R_u <=0)
JordanWisdom 0:4c63f2192998 236 DirR = 1;
JordanWisdom 0:4c63f2192998 237 else
JordanWisdom 0:4c63f2192998 238 DirR = 0;
JordanWisdom 0:4c63f2192998 239
JordanWisdom 0:4c63f2192998 240 PwmL.write(abs(L_u));
JordanWisdom 0:4c63f2192998 241 PwmR.write(abs(R_u));
JordanWisdom 0:4c63f2192998 242 }
JordanWisdom 0:4c63f2192998 243
JordanWisdom 0:4c63f2192998 244 /// ***************************************************
JordanWisdom 0:4c63f2192998 245 // SaturateAdd
JordanWisdom 0:4c63f2192998 246 // ***************************************************
JordanWisdom 0:4c63f2192998 247
JordanWisdom 0:4c63f2192998 248 signed int SaturateAdd(signed int x, signed int y){
JordanWisdom 0:4c63f2192998 249
JordanWisdom 0:4c63f2192998 250 signed int z = x + y;
JordanWisdom 0:4c63f2192998 251
JordanWisdom 0:4c63f2192998 252 if( (x>0) && (y>0)&& (z<=0) ){
JordanWisdom 0:4c63f2192998 253 z = 0x7FFFFFFF;}
JordanWisdom 0:4c63f2192998 254
JordanWisdom 0:4c63f2192998 255 else if( (x<0)&&(y<0)&&(z>=0) ){
JordanWisdom 0:4c63f2192998 256 z = 0x80000000;}
JordanWisdom 0:4c63f2192998 257
JordanWisdom 0:4c63f2192998 258 return z;
JordanWisdom 0:4c63f2192998 259 }
JordanWisdom 0:4c63f2192998 260
JordanWisdom 0:4c63f2192998 261 /// ***************************************************
JordanWisdom 0:4c63f2192998 262 // SaturateLimit
JordanWisdom 0:4c63f2192998 263 // ***************************************************
JordanWisdom 0:4c63f2192998 264
JordanWisdom 0:4c63f2192998 265 float SaturateLimit(float x, float limit){
JordanWisdom 0:4c63f2192998 266
JordanWisdom 0:4c63f2192998 267 if (x > limit){
JordanWisdom 0:4c63f2192998 268 return limit;
JordanWisdom 0:4c63f2192998 269 }
JordanWisdom 0:4c63f2192998 270 else if(x < -limit){
JordanWisdom 0:4c63f2192998 271 return(-limit);
JordanWisdom 0:4c63f2192998 272 }
JordanWisdom 0:4c63f2192998 273 else{
JordanWisdom 0:4c63f2192998 274 return x;}
JordanWisdom 0:4c63f2192998 275
JordanWisdom 0:4c63f2192998 276 }
JordanWisdom 0:4c63f2192998 277
JordanWisdom 0:4c63f2192998 278 /// ***************************************************
JordanWisdom 0:4c63f2192998 279 // Sign Extend
JordanWisdom 0:4c63f2192998 280 // ***************************************************
JordanWisdom 0:4c63f2192998 281
JordanWisdom 0:4c63f2192998 282 signed int SignExtend(int x){
JordanWisdom 0:4c63f2192998 283
JordanWisdom 0:4c63f2192998 284 if(x&0x00008000){
JordanWisdom 0:4c63f2192998 285 x = x|0xFFFF0000;
JordanWisdom 0:4c63f2192998 286 }
JordanWisdom 0:4c63f2192998 287
JordanWisdom 0:4c63f2192998 288 return x;
JordanWisdom 0:4c63f2192998 289 }
JordanWisdom 0:4c63f2192998 290
JordanWisdom 1:d523415c7b53 291 char GUI(){
JordanWisdom 1:d523415c7b53 292 char x;
JordanWisdom 1:d523415c7b53 293 pc.printf("\n\r (C)ircle or (L)ine? >>");
JordanWisdom 1:d523415c7b53 294 pc.scanf("%c", &x);
JordanWisdom 1:d523415c7b53 295 return x;
JordanWisdom 1:d523415c7b53 296 }
JordanWisdom 0:4c63f2192998 297
JordanWisdom 1:d523415c7b53 298 void Settings(char x){
JordanWisdom 1:d523415c7b53 299 char z;
JordanWisdom 1:d523415c7b53 300 if(x == 'c' || x == 'C')
JordanWisdom 1:d523415c7b53 301 {
JordanWisdom 1:d523415c7b53 302 pc.printf("\n\rCircle Radius? (m)");
JordanWisdom 1:d523415c7b53 303 pc.scanf("%f", &Radius);
JordanWisdom 1:d523415c7b53 304 pc.printf("\n\rDesired Speed? (m)");
JordanWisdom 1:d523415c7b53 305 pc.scanf("%f", &VelDes);
JordanWisdom 1:d523415c7b53 306 pc.printf("\n\r Desired Direction? (L) or (R)");
JordanWisdom 1:d523415c7b53 307 pc.scanf("%c",&z);
JordanWisdom 1:d523415c7b53 308 float Circ = 2*pi*Radius;
JordanWisdom 1:d523415c7b53 309 float tTot = Circ/VelDes;
JordanWisdom 1:d523415c7b53 310 if(z == 'l' || z == 'L')
JordanWisdom 1:d523415c7b53 311 {
JordanWisdom 1:d523415c7b53 312 I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
JordanWisdom 1:d523415c7b53 313 if(I_setpoint>35)
JordanWisdom 1:d523415c7b53 314 {
JordanWisdom 1:d523415c7b53 315 I_setpoint = 35;
JordanWisdom 1:d523415c7b53 316 printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
JordanWisdom 1:d523415c7b53 317 }
JordanWisdom 1:d523415c7b53 318 O_setpoint = I_setpoint*(Radius-L/2)/(Radius+L/2);
JordanWisdom 1:d523415c7b53 319 }
JordanWisdom 1:d523415c7b53 320 else
JordanWisdom 1:d523415c7b53 321 {
JordanWisdom 1:d523415c7b53 322 O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot);
JordanWisdom 1:d523415c7b53 323 if(O_setpoint>35)
JordanWisdom 1:d523415c7b53 324 {
JordanWisdom 1:d523415c7b53 325 O_setpoint = 35;
JordanWisdom 1:d523415c7b53 326 printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094);
JordanWisdom 1:d523415c7b53 327
JordanWisdom 1:d523415c7b53 328 }
JordanWisdom 1:d523415c7b53 329 I_setpoint = (Radius-L/2)*2*pi/((2*2.54/100)*tTot);
JordanWisdom 1:d523415c7b53 330 }
JordanWisdom 1:d523415c7b53 331 }
JordanWisdom 1:d523415c7b53 332 else
JordanWisdom 1:d523415c7b53 333 {
JordanWisdom 1:d523415c7b53 334 pc.printf("\n\r Desired Speed? (m) >>");
JordanWisdom 1:d523415c7b53 335 pc.scanf("%f", &VelDes);
JordanWisdom 1:d523415c7b53 336 O_setpoint = VelDes/0.05094;
JordanWisdom 1:d523415c7b53 337 if(O_setpoint>35)
JordanWisdom 1:d523415c7b53 338 {
JordanWisdom 1:d523415c7b53 339 O_setpoint = 35;
JordanWisdom 1:d523415c7b53 340 printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094);
JordanWisdom 1:d523415c7b53 341 }
JordanWisdom 1:d523415c7b53 342 I_setpoint = O_setpoint;
JordanWisdom 1:d523415c7b53 343 }
JordanWisdom 1:d523415c7b53 344
JordanWisdom 1:d523415c7b53 345
JordanWisdom 1:d523415c7b53 346 return;
JordanWisdom 1:d523415c7b53 347 }
JordanWisdom 0:4c63f2192998 348
JordanWisdom 0:4c63f2192998 349 // ==============================================================================================================
JordanWisdom 0:4c63f2192998 350 // ==============================================================================================================
JordanWisdom 0:4c63f2192998 351
JordanWisdom 0:4c63f2192998 352
JordanWisdom 0:4c63f2192998 353 // *********************************************************************
JordanWisdom 0:4c63f2192998 354 // MAIN FUNCTION
JordanWisdom 0:4c63f2192998 355 // *********************************************************************
JordanWisdom 0:4c63f2192998 356
JordanWisdom 0:4c63f2192998 357 int main(){
JordanWisdom 0:4c63f2192998 358
JordanWisdom 1:d523415c7b53 359 // Initialization
JordanWisdom 0:4c63f2192998 360
JordanWisdom 0:4c63f2192998 361 DE0_Init(0x8004);
JordanWisdom 0:4c63f2192998 362 L_MotorInit();
JordanWisdom 0:4c63f2192998 363 R_MotorInit();
JordanWisdom 0:4c63f2192998 364 L_integrator = 0;
JordanWisdom 0:4c63f2192998 365 R_integrator = 0;
JordanWisdom 0:4c63f2192998 366 ControlInterrupt.attach(&ControlThread, 0.0005);
JordanWisdom 0:4c63f2192998 367
JordanWisdom 0:4c63f2192998 368 //Motion Primitives Initialization
JordanWisdom 0:4c63f2192998 369 Arc = 0; // Arc Length (start at 0)
JordanWisdom 1:d523415c7b53 370 Settings(GUI());
JordanWisdom 0:4c63f2192998 371
JordanWisdom 0:4c63f2192998 372 // Display Global Variables to Console
JordanWisdom 0:4c63f2192998 373 while(1){
JordanWisdom 1:d523415c7b53 374 pc.printf("\n\r||Radius: %2.2f m||VLeft: %2.2f m/s ||VRight: %2.2f m/s ||Average: %2.2f m/s",Radius, O_setpoint*0.05094,I_setpoint*0.05094,VelDes);
JordanWisdom 0:4c63f2192998 375 wait(0.75);
JordanWisdom 1:d523415c7b53 376 }
JordanWisdom 0:4c63f2192998 377 }