E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@7:ea395616348c, 2015-03-02 (annotated)

Committer:
mawk2311
Date:
Mon Mar 02 22:11:35 2015 +0000
Revision:
7:ea395616348c
Parent:
6:78a2c2a7f39e
Child:
8:f3e0b4814888
Child:
9:aecffea74d88
Cleaned up the code and added "case a", which allows us to get the average value of 20, 30, and 50 percent duty cycles.

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
ericoneill 2:30ebae0d3e17 150 char choice = pc.getc();
mawk2311 5:61a0a21134f7 151 pc.putc(choice);
mawk2311 5:61a0a21134f7 152
ericoneill 2:30ebae0d3e17 153 switch(choice){
ericoneill 0:d328ecb3fbb1 154 case '0':
ericoneill 0:d328ecb3fbb1 155 motor.pulsewidth(0.0);
ericoneill 2:30ebae0d3e17 156 pc.printf("0% \n\r");
ericoneill 4:263bddc51c0f 157
ericoneill 0:d328ecb3fbb1 158 break;
ericoneill 0:d328ecb3fbb1 159 case '1':
ericoneill 0:d328ecb3fbb1 160 motor.pulsewidth(.0025);
ericoneill 2:30ebae0d3e17 161 pc.printf("100% \n\r");
ericoneill 4:263bddc51c0f 162 wait(.5);
mawk2311 7:ea395616348c 163 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 164
ericoneill 4:263bddc51c0f 165 break;
ericoneill 4:263bddc51c0f 166 case '2':
ericoneill 4:263bddc51c0f 167 motor.pulsewidth(.0025*.2);
mawk2311 7:ea395616348c 168 pc.printf("\n\r20% \n\r");
mawk2311 5:61a0a21134f7 169 wait(.5);
mawk2311 7:ea395616348c 170 pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 171
mawk2311 5:61a0a21134f7 172 while(1){
mawk2311 7:ea395616348c 173 velocity_control(0.2f, TUNING_CONSTANT_20);
mawk2311 5:61a0a21134f7 174 }
mawk2311 5:61a0a21134f7 175
mawk2311 7:ea395616348c 176 //break;
ericoneill 0:d328ecb3fbb1 177 case '3':
ericoneill 0:d328ecb3fbb1 178 motor.pulsewidth(.0025*.3);
mawk2311 7:ea395616348c 179 pc.printf("\n\r30% \n\r");
ericoneill 4:263bddc51c0f 180 wait(.5);
mawk2311 7:ea395616348c 181 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 182
mawk2311 5:61a0a21134f7 183 while(1){
mawk2311 7:ea395616348c 184 velocity_control(0.3f, TUNING_CONSTANT_30);
mawk2311 5:61a0a21134f7 185 }
mawk2311 5:61a0a21134f7 186
mawk2311 5:61a0a21134f7 187 //break;
ericoneill 0:d328ecb3fbb1 188 case '5':
ericoneill 0:d328ecb3fbb1 189 motor.pulsewidth(.0025*.5);
mawk2311 7:ea395616348c 190 pc.printf("\n\r50% \n\r");
ericoneill 4:263bddc51c0f 191 wait(.5);
mawk2311 7:ea395616348c 192 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 193
mawk2311 5:61a0a21134f7 194 while(1){
mawk2311 7:ea395616348c 195 velocity_control(0.5f, TUNING_CONSTANT_50);
mawk2311 5:61a0a21134f7 196 }
mawk2311 5:61a0a21134f7 197
mawk2311 5:61a0a21134f7 198 //break;
mawk2311 7:ea395616348c 199 case 'a':
mawk2311 7:ea395616348c 200 pc.printf("\n\rGet average velocity of which duty cycle?\n\r");
mawk2311 7:ea395616348c 201 choice = pc.getc();
mawk2311 7:ea395616348c 202 pc.putc(choice);
mawk2311 7:ea395616348c 203
mawk2311 7:ea395616348c 204 switch(choice){
mawk2311 7:ea395616348c 205
mawk2311 7:ea395616348c 206 case '2':
mawk2311 7:ea395616348c 207 motor.pulsewidth(.0025*0.2f);
mawk2311 7:ea395616348c 208 pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 209 break;
mawk2311 7:ea395616348c 210
mawk2311 7:ea395616348c 211 case '3':
mawk2311 7:ea395616348c 212 motor.pulsewidth(.0025*0.3f);
mawk2311 7:ea395616348c 213 pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 214 break;
mawk2311 7:ea395616348c 215
mawk2311 7:ea395616348c 216 case '5':
mawk2311 7:ea395616348c 217 motor.pulsewidth(.0025*0.5f);
mawk2311 7:ea395616348c 218 pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 219 break;
mawk2311 7:ea395616348c 220
mawk2311 7:ea395616348c 221 default:
mawk2311 7:ea395616348c 222 break;
mawk2311 7:ea395616348c 223 }
mawk2311 7:ea395616348c 224 break;
mawk2311 7:ea395616348c 225
ericoneill 0:d328ecb3fbb1 226 default:
mawk2311 7:ea395616348c 227 motor.pulsewidth(.0025*0);
ericoneill 2:30ebae0d3e17 228 pc.printf("default\n\r");
ericoneill 0:d328ecb3fbb1 229 break;
ericoneill 0:d328ecb3fbb1 230 }
ericoneill 3:7eaf505f811e 231
ericoneill 0:d328ecb3fbb1 232 //servo_sweep();
ericoneill 0:d328ecb3fbb1 233 }
ericoneill 0:d328ecb3fbb1 234 }