Diff: main.cpp

Revision:

4:263bddc51c0f

Parent:

3:7eaf505f811e

Child:

5:61a0a21134f7

--- a/main.cpp	Fri Feb 27 02:18:17 2015 +0000
+++ b/main.cpp	Fri Feb 27 04:19:03 2015 +0000
@@ -7,10 +7,27 @@
 
 // encoder setup and variables
 InterruptIn interrupt(PTA13);
-int previous_val = -1;
+
+int interval1=0;
+int interval2=0;
+int interval3=0;
+int avg_interval=0;
+int lastchange1 = 0;
+int lastchange2 = 0;
+int lastchange3 = 0;
+int lastchange4 = 0;
 Timer t;
+//constant *(PWM-expected)
+const float TUNING_CONSTANT = .5;
+const float PI = 3.14159;
+const float WHEEL_CIRCUMFERENCE = .05*PI;
 
-
+float get_speed(){
+    float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
+    float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
+    return linearSpeed;
+    
+}
 void servo_sweep(){
     for(float p = 0.001; p<0.002; p+=0.0001){
             servo.pulsewidth(p);
@@ -18,10 +35,33 @@
     }
 }
 void fallInterrupt(){
-    pc.printf("fall");
+    
+    int time = t.read_us();
+    interval1 = time - lastchange2;
+    interval2 = lastchange1-lastchange3;
+    interval3 = lastchange2 - lastchange4;
+    avg_interval = (interval1 + interval2 + interval3)/3;
+    
+    lastchange4 = lastchange3;
+    lastchange3 = lastchange2;
+    lastchange2 = lastchange1;
+    lastchange1 = time;
+    //pc.printf("dark to light time : %d\n\r", interval);
+    //pc.printf("fall");
 }
 void riseInterrupt(){
-    pc.printf("rise");
+    int time = t.read_us();
+    interval1 = time - lastchange2;
+    interval2 = lastchange1-lastchange3;
+    interval3 = lastchange2 - lastchange4;
+    avg_interval = (interval1 + interval2 + interval3)/3;
+    
+    lastchange4 = lastchange3;
+    lastchange3 = lastchange2;
+    lastchange2 = lastchange1;
+    lastchange1 = time;
+    //pc.printf("light to dark time : %d\n\r", interval);
+    //pc.printf("rise");
 }
     
 int main() {
@@ -32,31 +72,7 @@
     
     
     t.start();
-    int lastchange = 0;
-    while(1){/*
-        if(din) {
-            if(previous_val != 1){
-                int current_time = t.read_ms();
-                int interval = current_time - lastchange;
-                lastchange = current_time;
-                pc.printf("light to dark time : %d\n\r", interval);
-                previous_val = 1;
-            }
-            myled = 1;
-            //pc.printf("dark");
-        } else {
-            if(previous_val != 0){
-                int current_time = t.read_ms();
-                int interval = current_time - lastchange;
-                lastchange = current_time;
-                pc.printf("dark to light time : %d\n\r", interval);
-                previous_val = 0;
-            }
-            myled = 0;
-            //pc.printf("light");
-        }
-        //wait(.2f);
-        */
+    while(1){
         
         char choice = pc.getc();
         pc.putc(choice);
@@ -64,18 +80,31 @@
             case '0':
                 motor.pulsewidth(0.0);
                 pc.printf("0% \n\r");
+                
                 break;
             case '1':
                 motor.pulsewidth(.0025);
                 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;
             case '3':
                 motor.pulsewidth(.0025*.3);
                 pc.printf("30% \n\r");
+                wait(.5);
+                pc.printf("speed: %f",get_speed());
                 break;
             case '5':
                 motor.pulsewidth(.0025*.5);
                 pc.printf("50% \n\r");
+                wait(.5);
+                pc.printf("speed: %f",get_speed());
                 break;
             default:
                 motor.pulsewidth(.0025*.3);
@@ -84,7 +113,5 @@
         }
     
        //servo_sweep();
-       //motor_sweep();
-       //motor.pulsewidth(.0025);
     }
 }