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;
}