E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@10:de7a56fb94bc, 2015-03-06 (annotated)

Committer:
mawk2311
Date:
Fri Mar 06 06:01:27 2015 +0000
Revision:
10:de7a56fb94bc
Parent:
8:f3e0b4814888
Child:
11:d07a4a683289
servo code working

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