E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@9:aecffea74d88, 2015-03-06 (annotated)

Committer:
mawk2311
Date:
Fri Mar 06 05:51:01 2015 +0000
Revision:
9:aecffea74d88
Parent:
7:ea395616348c 
michael changes

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