E=MC
/
e=mc
.
Diff: main.cpp
- Revision:
- 5:61a0a21134f7
- Parent:
- 4:263bddc51c0f
- Child:
- 6:78a2c2a7f39e
diff -r 263bddc51c0f -r 61a0a21134f7 main.cpp --- a/main.cpp Fri Feb 27 04:19:03 2015 +0000 +++ b/main.cpp Fri Feb 27 06:36:42 2015 +0000 @@ -16,9 +16,15 @@ int lastchange2 = 0; int lastchange3 = 0; int lastchange4 = 0; + +float avg_speed_list [100]; +float small_avg_speed_list [10]; + Timer t; //constant *(PWM-expected) -const float TUNING_CONSTANT = .5; +const float TUNING_CONSTANT_20 = 3.697; +const float TUNING_CONSTANT_30 = 5.607; +const float TUNING_CONSTANT_50 = 6.880; const float PI = 3.14159; const float WHEEL_CIRCUMFERENCE = .05*PI; @@ -28,6 +34,23 @@ return linearSpeed; } + +float get_avg_speed() { + + 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 < 10; c++) { + avg_avg_speed += small_avg_speed_list[c]; + } + + return avg_avg_speed/10.0f; +} + void servo_sweep(){ for(float p = 0.001; p<0.002; p+=0.0001){ servo.pulsewidth(p); @@ -74,8 +97,11 @@ t.start(); while(1){ + float avg_speed = 0; + char choice = pc.getc(); - pc.putc(choice); + pc.putc(choice); + switch(choice){ case '0': motor.pulsewidth(0.0); @@ -87,25 +113,79 @@ pc.printf("100% \n\r"); wait(.5); pc.printf("speed: %f",get_speed()); + break; case '2': motor.pulsewidth(.0025*.2); pc.printf("20% \n\r"); - wait(.5); - pc.printf("speed: %f",get_speed()); - break; + wait(.5); + float tuning_val = 1; + pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed()); + + while(1){ + + avg_speed = get_avg_speed(); + + if((avg_speed - TUNING_CONSTANT_20) > 0.5f){ + tuning_val -= .1; + } else if (avg_speed - TUNING_CONSTANT_20 < -0.5f){ + tuning_val += .1; + } else { + tuning_val = 1; + } + motor.pulsewidth(.0025 * .2 * tuning_val); + pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); + wait(.5); + } + + //break; + case '3': motor.pulsewidth(.0025*.3); pc.printf("30% \n\r"); wait(.5); - pc.printf("speed: %f",get_speed()); - break; + pc.printf("speed: %f",get_avg_speed()); + + 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); + } + + //break; case '5': motor.pulsewidth(.0025*.5); pc.printf("50% \n\r"); wait(.5); - pc.printf("speed: %f",get_speed()); - break; + pc.printf("speed: %f",get_avg_speed()); + + 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); + } + + //break; default: motor.pulsewidth(.0025*.3); pc.printf("default\n\r");