Diff: main.cpp

Revision:

5:61a0a21134f7

Parent:

4:263bddc51c0f

Child:

6:78a2c2a7f39e

--- 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");