E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@12:46d0ff953a3f, 2015-03-20 (annotated)

Committer:
mawk2311
Date:
Fri Mar 20 01:29:47 2015 +0000
Revision:
12:46d0ff953a3f
Parent:
11:d07a4a683289
Child:
14:bda4a189cbe8
Second Motor incorporated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericoneill 0:d328ecb3fbb1 1 #include "mbed.h"
ericoneill 0:d328ecb3fbb1 2
ericoneill 0:d328ecb3fbb1 3 DigitalOut myled(LED1);
ericoneill 0:d328ecb3fbb1 4 PwmOut servo(PTA5);
mawk2311 11:d07a4a683289 5 PwmOut motor1(PTA4);
mawk2311 11:d07a4a683289 6 PwmOut motor2(PTA12);
mawk2311 12:46d0ff953a3f 7 DigitalOut break1(PTC12);
mawk2311 12:46d0ff953a3f 8 DigitalOut break2(PTC13);
mawk2311 12:46d0ff953a3f 9
ericoneill 0:d328ecb3fbb1 10 Serial pc(USBTX, USBRX); // tx, rx
ericoneill 3:7eaf505f811e 11
ericoneill 3:7eaf505f811e 12 // encoder setup and variables
ericoneill 3:7eaf505f811e 13 InterruptIn interrupt(PTA13);
ericoneill 4:263bddc51c0f 14
mawk2311 7:ea395616348c 15 //Intervals used during encoder data collection to measure velocity
ericoneill 4:263bddc51c0f 16 int interval1=0;
ericoneill 4:263bddc51c0f 17 int interval2=0;
ericoneill 4:263bddc51c0f 18 int interval3=0;
ericoneill 4:263bddc51c0f 19 int avg_interval=0;
ericoneill 4:263bddc51c0f 20 int lastchange1 = 0;
ericoneill 4:263bddc51c0f 21 int lastchange2 = 0;
ericoneill 4:263bddc51c0f 22 int lastchange3 = 0;
ericoneill 4:263bddc51c0f 23 int lastchange4 = 0;
mawk2311 5:61a0a21134f7 24
mawk2311 7:ea395616348c 25 //Variables used to for velocity control
mawk2311 7:ea395616348c 26 float avg_speed = 0;
mawk2311 7:ea395616348c 27 float stall_check = 0;
mawk2311 7:ea395616348c 28 float tuning_val = 1;
mawk2311 5:61a0a21134f7 29
ericoneill 1:8e5821dec0f7 30 Timer t;
ericoneill 8:f3e0b4814888 31 Timer servoTimer;
ericoneill 8:f3e0b4814888 32
ericoneill 8:f3e0b4814888 33 // Servo parameters
ericoneill 8:f3e0b4814888 34 float lastTurnTime = 0.0f;
ericoneill 8:f3e0b4814888 35 bool servoLeft = true;
mawk2311 7:ea395616348c 36
mawk2311 7:ea395616348c 37 //Observed average speeds for each duty cycle
mawk2311 12:46d0ff953a3f 38 const float TUNING_CONSTANT_10 = 1.10;
mawk2311 7:ea395616348c 39 const float TUNING_CONSTANT_20 = 3.00;
mawk2311 7:ea395616348c 40 const float TUNING_CONSTANT_30 = 4.30;
mawk2311 5:61a0a21134f7 41 const float TUNING_CONSTANT_50 = 6.880;
ericoneill 4:263bddc51c0f 42 const float PI = 3.14159;
ericoneill 4:263bddc51c0f 43 const float WHEEL_CIRCUMFERENCE = .05*PI;
ericoneill 0:d328ecb3fbb1 44
mawk2311 7:ea395616348c 45 //Velocity Control Tuning Constants
mawk2311 7:ea395616348c 46 const float TUNE_THRESH = 0.5f;
mawk2311 7:ea395616348c 47 const float TUNE_AMT = 0.1f;
mawk2311 7:ea395616348c 48
mawk2311 7:ea395616348c 49 //Parameters specifying sample sizes and delays for small and large average speed samples
mawk2311 7:ea395616348c 50 float num_samples_small = 10.0f;
mawk2311 7:ea395616348c 51 float delay_small = 0.05f;
mawk2311 7:ea395616348c 52 float num_samples_large = 100.0f;
mawk2311 7:ea395616348c 53 float delay_large = 0.1f;
mawk2311 7:ea395616348c 54
mawk2311 7:ea395616348c 55 // Large and small arrays used to get average velocity values
mawk2311 7:ea395616348c 56 float large_avg_speed_list [100];
mawk2311 7:ea395616348c 57 float small_avg_speed_list [10];
mawk2311 7:ea395616348c 58
mawk2311 7:ea395616348c 59 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ericoneill 4:263bddc51c0f 60 float get_speed(){
ericoneill 4:263bddc51c0f 61 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
ericoneill 4:263bddc51c0f 62 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
ericoneill 4:263bddc51c0f 63 return linearSpeed;
ericoneill 4:263bddc51c0f 64 }
mawk2311 5:61a0a21134f7 65
mawk2311 7:ea395616348c 66 float get_avg_speed(float num_samples, float delay) {
mawk2311 5:61a0a21134f7 67
mawk2311 5:61a0a21134f7 68 float avg_avg_speed = 0;
mawk2311 5:61a0a21134f7 69
mawk2311 7:ea395616348c 70 for (int c = 0; c < num_samples; c++) {
mawk2311 7:ea395616348c 71 if (num_samples == num_samples_small){
mawk2311 7:ea395616348c 72 small_avg_speed_list[c] = get_speed();
mawk2311 7:ea395616348c 73 } else if (num_samples == num_samples_large){
mawk2311 7:ea395616348c 74 large_avg_speed_list[c] = get_speed();
mawk2311 7:ea395616348c 75 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
mawk2311 7:ea395616348c 76 }
mawk2311 7:ea395616348c 77 wait(delay);
mawk2311 5:61a0a21134f7 78 }
mawk2311 5:61a0a21134f7 79
mawk2311 7:ea395616348c 80 for (int c = 1; c < num_samples; c++) {
mawk2311 7:ea395616348c 81 if (num_samples == num_samples_small){
mawk2311 7:ea395616348c 82 avg_avg_speed += small_avg_speed_list[c];
mawk2311 7:ea395616348c 83 } else if (num_samples == num_samples_large){
mawk2311 7:ea395616348c 84 avg_avg_speed += large_avg_speed_list[c];
mawk2311 7:ea395616348c 85 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
mawk2311 7:ea395616348c 86 }
mawk2311 5:61a0a21134f7 87 }
mawk2311 7:ea395616348c 88 return avg_avg_speed/num_samples;
mawk2311 5:61a0a21134f7 89 }
mawk2311 5:61a0a21134f7 90
mawk2311 7:ea395616348c 91 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 7:ea395616348c 92
mawk2311 7:ea395616348c 93 avg_speed = get_avg_speed(num_samples_small, delay_small);
mawk2311 7:ea395616348c 94
mawk2311 7:ea395616348c 95 if (avg_speed == stall_check) {
mawk2311 7:ea395616348c 96 avg_speed = 0;
mawk2311 7:ea395616348c 97 tuning_val += TUNE_AMT;
mawk2311 12:46d0ff953a3f 98 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 7:ea395616348c 99 tuning_val -= TUNE_AMT;
mawk2311 7:ea395616348c 100 stall_check = avg_speed;
mawk2311 7:ea395616348c 101 } else if (avg_speed - tuning_const < -1*TUNE_THRESH){
mawk2311 7:ea395616348c 102 tuning_val += TUNE_AMT;
mawk2311 7:ea395616348c 103 stall_check = avg_speed;
mawk2311 7:ea395616348c 104 } else {
mawk2311 12:46d0ff953a3f 105 //tuning_val = 1;
mawk2311 7:ea395616348c 106 stall_check = avg_speed;
mawk2311 7:ea395616348c 107 }
mawk2311 11:d07a4a683289 108 motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 11:d07a4a683289 109 motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 7:ea395616348c 110
mawk2311 7:ea395616348c 111 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 7:ea395616348c 112 wait(.2);
mawk2311 7:ea395616348c 113 }
mawk2311 7:ea395616348c 114
ericoneill 0:d328ecb3fbb1 115 void servo_sweep(){
ericoneill 0:d328ecb3fbb1 116 for(float p = 0.001; p<0.002; p+=0.0001){
ericoneill 0:d328ecb3fbb1 117 servo.pulsewidth(p);
ericoneill 0:d328ecb3fbb1 118 wait(0.5);
ericoneill 0:d328ecb3fbb1 119 }
ericoneill 0:d328ecb3fbb1 120 }
mawk2311 7:ea395616348c 121
ericoneill 3:7eaf505f811e 122 void fallInterrupt(){
ericoneill 4:263bddc51c0f 123
ericoneill 4:263bddc51c0f 124 int time = t.read_us();
ericoneill 4:263bddc51c0f 125 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 126 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 127 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 128 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 129
ericoneill 4:263bddc51c0f 130 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 131 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 132 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 133 lastchange1 = time;
ericoneill 4:263bddc51c0f 134 //pc.printf("dark to light time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 135 //pc.printf("fall");
ericoneill 3:7eaf505f811e 136 }
ericoneill 3:7eaf505f811e 137 void riseInterrupt(){
ericoneill 4:263bddc51c0f 138 int time = t.read_us();
ericoneill 4:263bddc51c0f 139 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 140 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 141 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 142 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 143
ericoneill 4:263bddc51c0f 144 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 145 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 146 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 147 lastchange1 = time;
ericoneill 4:263bddc51c0f 148 //pc.printf("light to dark time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 149 //pc.printf("rise");
ericoneill 3:7eaf505f811e 150 }
ericoneill 3:7eaf505f811e 151
ericoneill 0:d328ecb3fbb1 152 int main() {
ericoneill 0:d328ecb3fbb1 153 servo.period(0.005);
mawk2311 11:d07a4a683289 154 motor1.period(.0025);
mawk2311 11:d07a4a683289 155 motor2.period(.0025);
ericoneill 3:7eaf505f811e 156 interrupt.fall(&fallInterrupt);
ericoneill 3:7eaf505f811e 157 interrupt.rise(&riseInterrupt);
ericoneill 3:7eaf505f811e 158
ericoneill 1:8e5821dec0f7 159 t.start();
ericoneill 4:263bddc51c0f 160 while(1){
ericoneill 2:30ebae0d3e17 161
mawk2311 12:46d0ff953a3f 162 wait(3);
mawk2311 12:46d0ff953a3f 163 char choice = '1'; //pc.getc();
mawk2311 5:61a0a21134f7 164 pc.putc(choice);
mawk2311 5:61a0a21134f7 165
ericoneill 2:30ebae0d3e17 166 switch(choice){
ericoneill 0:d328ecb3fbb1 167 case '0':
mawk2311 11:d07a4a683289 168 motor1.pulsewidth(0.0);
mawk2311 11:d07a4a683289 169 motor2.pulsewidth(0.0);
ericoneill 2:30ebae0d3e17 170 pc.printf("0% \n\r");
ericoneill 4:263bddc51c0f 171
ericoneill 0:d328ecb3fbb1 172 break;
ericoneill 0:d328ecb3fbb1 173 case '1':
mawk2311 12:46d0ff953a3f 174 motor1.pulsewidth(.0025*.1);
mawk2311 12:46d0ff953a3f 175 motor2.pulsewidth(.0025*.1);
mawk2311 12:46d0ff953a3f 176 break1 = 0;
mawk2311 12:46d0ff953a3f 177 break2 = 0;
ericoneill 2:30ebae0d3e17 178 pc.printf("100% \n\r");
ericoneill 4:263bddc51c0f 179 wait(.5);
mawk2311 7:ea395616348c 180 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 181
mawk2311 12:46d0ff953a3f 182 while(1){
mawk2311 12:46d0ff953a3f 183 velocity_control(0.1f, TUNING_CONSTANT_10);
mawk2311 12:46d0ff953a3f 184 }
mawk2311 12:46d0ff953a3f 185
mawk2311 12:46d0ff953a3f 186 //break;
ericoneill 4:263bddc51c0f 187 case '2':
mawk2311 11:d07a4a683289 188 motor1.pulsewidth(.0025*.2);
mawk2311 11:d07a4a683289 189 motor2.pulsewidth(.0025*.2);
mawk2311 12:46d0ff953a3f 190 break1 = 0;
mawk2311 12:46d0ff953a3f 191 break2 = 0;
mawk2311 7:ea395616348c 192 pc.printf("\n\r20% \n\r");
mawk2311 5:61a0a21134f7 193 wait(.5);
mawk2311 7:ea395616348c 194 pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small));
mawk2311 10:de7a56fb94bc 195 servoTimer.start();
mawk2311 5:61a0a21134f7 196 while(1){
mawk2311 10:de7a56fb94bc 197
mawk2311 7:ea395616348c 198 velocity_control(0.2f, TUNING_CONSTANT_20);
ericoneill 8:f3e0b4814888 199 if(servoLeft){
ericoneill 8:f3e0b4814888 200 servo.pulsewidth(.001);
ericoneill 8:f3e0b4814888 201 }
ericoneill 8:f3e0b4814888 202 else{
ericoneill 8:f3e0b4814888 203 servo.pulsewidth(.002);
ericoneill 8:f3e0b4814888 204 }
ericoneill 8:f3e0b4814888 205 float turnTime = servoTimer.read();
ericoneill 8:f3e0b4814888 206 if(turnTime - lastTurnTime > 3.0){
ericoneill 8:f3e0b4814888 207 servoLeft = !servoLeft;
ericoneill 8:f3e0b4814888 208 lastTurnTime = turnTime;
ericoneill 8:f3e0b4814888 209 }
mawk2311 5:61a0a21134f7 210 }
mawk2311 5:61a0a21134f7 211
mawk2311 7:ea395616348c 212 //break;
ericoneill 0:d328ecb3fbb1 213 case '3':
mawk2311 11:d07a4a683289 214 motor1.pulsewidth(.0025*.3);
mawk2311 11:d07a4a683289 215 motor2.pulsewidth(.0025*.3);
mawk2311 12:46d0ff953a3f 216 break1 = 0;
mawk2311 12:46d0ff953a3f 217 break2 = 0;
mawk2311 7:ea395616348c 218 pc.printf("\n\r30% \n\r");
ericoneill 4:263bddc51c0f 219 wait(.5);
mawk2311 7:ea395616348c 220 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 221
mawk2311 5:61a0a21134f7 222 while(1){
mawk2311 7:ea395616348c 223 velocity_control(0.3f, TUNING_CONSTANT_30);
mawk2311 5:61a0a21134f7 224 }
mawk2311 5:61a0a21134f7 225
mawk2311 5:61a0a21134f7 226 //break;
ericoneill 0:d328ecb3fbb1 227 case '5':
mawk2311 11:d07a4a683289 228 motor1.pulsewidth(.0025*.5);
mawk2311 11:d07a4a683289 229 motor2.pulsewidth(.0025*.5);
mawk2311 12:46d0ff953a3f 230 break1 = 0;
mawk2311 12:46d0ff953a3f 231 break2 = 0;
mawk2311 7:ea395616348c 232 pc.printf("\n\r50% \n\r");
ericoneill 4:263bddc51c0f 233 wait(.5);
mawk2311 7:ea395616348c 234 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 235
mawk2311 5:61a0a21134f7 236 while(1){
mawk2311 7:ea395616348c 237 velocity_control(0.5f, TUNING_CONSTANT_50);
mawk2311 5:61a0a21134f7 238 }
mawk2311 5:61a0a21134f7 239
mawk2311 5:61a0a21134f7 240 //break;
mawk2311 7:ea395616348c 241 case 'a':
mawk2311 7:ea395616348c 242 pc.printf("\n\rGet average velocity of which duty cycle?\n\r");
mawk2311 7:ea395616348c 243 choice = pc.getc();
mawk2311 7:ea395616348c 244 pc.putc(choice);
mawk2311 7:ea395616348c 245
mawk2311 12:46d0ff953a3f 246 break1 = 0;
mawk2311 12:46d0ff953a3f 247 break2 = 0;
mawk2311 7:ea395616348c 248 switch(choice){
mawk2311 7:ea395616348c 249
mawk2311 12:46d0ff953a3f 250 case '1':
mawk2311 12:46d0ff953a3f 251 motor1.pulsewidth(.0025*0.1f);
mawk2311 12:46d0ff953a3f 252 motor2.pulsewidth(.0025*0.1f);
mawk2311 12:46d0ff953a3f 253 pc.printf("\n\rLongterm average speed at 10 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 12:46d0ff953a3f 254 break;
mawk2311 12:46d0ff953a3f 255
mawk2311 7:ea395616348c 256 case '2':
mawk2311 11:d07a4a683289 257 motor1.pulsewidth(.0025*0.2f);
mawk2311 11:d07a4a683289 258 motor2.pulsewidth(.0025*0.2f);
mawk2311 7:ea395616348c 259 pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 260 break;
mawk2311 7:ea395616348c 261
mawk2311 7:ea395616348c 262 case '3':
mawk2311 11:d07a4a683289 263 motor1.pulsewidth(.0025*0.3f);
mawk2311 11:d07a4a683289 264 motor2.pulsewidth(.0025*0.3f);
mawk2311 7:ea395616348c 265 pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 266 break;
mawk2311 7:ea395616348c 267
mawk2311 7:ea395616348c 268 case '5':
mawk2311 11:d07a4a683289 269 motor1.pulsewidth(.0025*0.5f);
mawk2311 11:d07a4a683289 270 motor2.pulsewidth(.0025*0.5f);
mawk2311 7:ea395616348c 271 pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 272 break;
mawk2311 7:ea395616348c 273
mawk2311 7:ea395616348c 274 default:
mawk2311 7:ea395616348c 275 break;
mawk2311 7:ea395616348c 276 }
mawk2311 7:ea395616348c 277 break;
mawk2311 7:ea395616348c 278
ericoneill 0:d328ecb3fbb1 279 default:
mawk2311 11:d07a4a683289 280 motor1.pulsewidth(.0025*0);
mawk2311 11:d07a4a683289 281 motor2.pulsewidth(.0025*0);
mawk2311 12:46d0ff953a3f 282 break1 = 0;
mawk2311 12:46d0ff953a3f 283 break2 = 0;
ericoneill 2:30ebae0d3e17 284 pc.printf("default\n\r");
ericoneill 0:d328ecb3fbb1 285 break;
ericoneill 0:d328ecb3fbb1 286 }
ericoneill 3:7eaf505f811e 287
ericoneill 0:d328ecb3fbb1 288 //servo_sweep();
ericoneill 0:d328ecb3fbb1 289 }
ericoneill 0:d328ecb3fbb1 290 }