E=MC
/
e=mc
.
Diff: main.cpp
- Revision:
- 7:ea395616348c
- Parent:
- 6:78a2c2a7f39e
- Child:
- 8:f3e0b4814888
- Child:
- 9:aecffea74d88
--- a/main.cpp Mon Mar 02 19:38:38 2015 +0000 +++ b/main.cpp Mon Mar 02 22:11:35 2015 +0000 @@ -8,6 +8,7 @@ // encoder setup and variables InterruptIn interrupt(PTA13); +//Intervals used during encoder data collection to measure velocity int interval1=0; int interval2=0; int interval3=0; @@ -17,46 +18,96 @@ int lastchange3 = 0; int lastchange4 = 0; -float avg_speed_list [100]; -float small_avg_speed_list [10]; +//Variables used to for velocity control +float avg_speed = 0; +float stall_check = 0; +float tuning_val = 1; Timer t; -//constant *(PWM-expected) -const float TUNING_CONSTANT_20 = 3.697; -const float TUNING_CONSTANT_30 = 5.607; + +//Observed average speeds for each duty cycle +const float TUNING_CONSTANT_20 = 3.00; +const float TUNING_CONSTANT_30 = 4.30; const float TUNING_CONSTANT_50 = 6.880; const float PI = 3.14159; const float WHEEL_CIRCUMFERENCE = .05*PI; +//Velocity Control Tuning Constants +const float TUNE_THRESH = 0.5f; +const float TUNE_AMT = 0.1f; + +//Parameters specifying sample sizes and delays for small and large average speed samples +float num_samples_small = 10.0f; +float delay_small = 0.05f; +float num_samples_large = 100.0f; +float delay_large = 0.1f; + +// Large and small arrays used to get average velocity values +float large_avg_speed_list [100]; +float small_avg_speed_list [10]; + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ float get_speed(){ float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; return linearSpeed; - } -float get_avg_speed() { +float get_avg_speed(float num_samples, float delay) { float avg_avg_speed = 0; - for (int c = 0; c < 10; c++) { - small_avg_speed_list[c] = get_speed(); - wait(.05); + for (int c = 0; c < num_samples; c++) { + if (num_samples == num_samples_small){ + small_avg_speed_list[c] = get_speed(); + } else if (num_samples == num_samples_large){ + large_avg_speed_list[c] = get_speed(); + //pc.printf("\n\rworking: %f", large_avg_speed_list[c]); + } + wait(delay); } - for (int c = 0; c < 10; c++) { - avg_avg_speed += small_avg_speed_list[c]; + for (int c = 1; c < num_samples; c++) { + if (num_samples == num_samples_small){ + avg_avg_speed += small_avg_speed_list[c]; + } else if (num_samples == num_samples_large){ + avg_avg_speed += large_avg_speed_list[c]; + //pc.printf("\n\rworking: %f", large_avg_speed_list[c]); + } } - - return avg_avg_speed/10.0f; + return avg_avg_speed/num_samples; } +void velocity_control(float duty_cyc, float tuning_const) { + + avg_speed = get_avg_speed(num_samples_small, delay_small); + + if (avg_speed == stall_check) { + avg_speed = 0; + tuning_val += TUNE_AMT; + } else if((avg_speed - tuning_const) > TUNE_THRESH){ + tuning_val -= TUNE_AMT; + stall_check = avg_speed; + } else if (avg_speed - tuning_const < -1*TUNE_THRESH){ + tuning_val += TUNE_AMT; + stall_check = avg_speed; + } else { + tuning_val = 1; + stall_check = avg_speed; + } + motor.pulsewidth(.0025 * duty_cyc * tuning_val); + + pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); + wait(.2); +} + void servo_sweep(){ for(float p = 0.001; p<0.002; p+=0.0001){ servo.pulsewidth(p); wait(0.5); } } + void fallInterrupt(){ int time = t.read_us(); @@ -93,13 +144,9 @@ interrupt.fall(&fallInterrupt); interrupt.rise(&riseInterrupt); - t.start(); while(1){ - float avg_speed; - float stall_check; - char choice = pc.getc(); pc.putc(choice); @@ -113,88 +160,71 @@ motor.pulsewidth(.0025); pc.printf("100% \n\r"); wait(.5); - pc.printf("speed: %f",get_speed()); + pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); break; case '2': motor.pulsewidth(.0025*.2); - pc.printf("20% \n\r"); + pc.printf("\n\r20% \n\r"); wait(.5); - float tuning_val = 1; - pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed()); + pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small)); while(1){ - - avg_speed = get_avg_speed(); - if (avg_speed == stall_check) { - avg_speed = 0; - tuning_val += .1; - } else if((avg_speed - TUNING_CONSTANT_20) > 0.5f){ - tuning_val -= .1; - stall_check = avg_speed; - } else if (avg_speed - TUNING_CONSTANT_20 < -0.5f){ - tuning_val += .1; - stall_check = avg_speed; - } else { - tuning_val = 1; - stall_check = avg_speed; - } - motor.pulsewidth(.0025 * .2 * tuning_val); - - pc.printf("speed: %f\n\rtuning val: %f\n\r PWM : ", avg_speed, tuning_val); - wait(.5); + velocity_control(0.2f, TUNING_CONSTANT_20); } - //break; - + //break; case '3': motor.pulsewidth(.0025*.3); - pc.printf("30% \n\r"); + pc.printf("\n\r30% \n\r"); wait(.5); - pc.printf("speed: %f",get_avg_speed()); + pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); while(1){ - - avg_speed = get_avg_speed(); - - if((avg_speed - TUNING_CONSTANT_30) > 0.5f){ - tuning_val -= .1; - } else if (avg_speed - TUNING_CONSTANT_30 < -0.5f){ - tuning_val += .1; - } else { - tuning_val = 1; - } - motor.pulsewidth(.0025 * .3 * tuning_val); - pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); - wait(.5); + velocity_control(0.3f, TUNING_CONSTANT_30); } //break; case '5': motor.pulsewidth(.0025*.5); - pc.printf("50% \n\r"); + pc.printf("\n\r50% \n\r"); wait(.5); - pc.printf("speed: %f",get_avg_speed()); + pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); while(1){ - - avg_speed = get_avg_speed(); - - if((avg_speed - TUNING_CONSTANT_50) > 0.5f){ - tuning_val -= .1; - } else if (avg_speed - TUNING_CONSTANT_50 < -0.5f){ - tuning_val += .1; - } else { - tuning_val = 1; - } - motor.pulsewidth(.0025 * .5 * tuning_val); - pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); - wait(.5); + velocity_control(0.5f, TUNING_CONSTANT_50); } //break; + case 'a': + pc.printf("\n\rGet average velocity of which duty cycle?\n\r"); + choice = pc.getc(); + pc.putc(choice); + + switch(choice){ + + case '2': + motor.pulsewidth(.0025*0.2f); + pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); + break; + + case '3': + motor.pulsewidth(.0025*0.3f); + pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); + break; + + case '5': + motor.pulsewidth(.0025*0.5f); + pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); + break; + + default: + break; + } + break; + default: - motor.pulsewidth(.0025*.3); + motor.pulsewidth(.0025*0); pc.printf("default\n\r"); break; }