Diff: main.cpp

Revision:

2:2d32a0543c63

Parent:

1:9d05c0236c7e

--- a/main.cpp	Wed Oct 09 06:49:17 2013 +0000
+++ b/main.cpp	Thu Oct 31 15:22:46 2013 +0000
@@ -1,46 +1,74 @@
 #include "mbed.h"
-#include "encoder.h"
 #include "MODSERIAL.h"
 
+
 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
 void keep_in_range(float * in, float min, float max);
 
 volatile bool looptimerflag;
+float sluis10;
+int sluis11;
+float y;
+float y1;
+float y2;
+float z;
+float z1;
+float z2;
+float numl1;
+float numl2;
+float numl3;
+float denl1;
+float denl2;
+float denl3;
 
 void setlooptimerflag(void)
 {
     looptimerflag = true;
 }
-
+    AnalogIn sluis1(PTC2);
+    
 
 int main() {
-    //LOCAL VARIABLES 
-    AnalogIn potmeter(PTC2);
-    Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx
+        //START OF CODE
     MODSERIAL pc(USBTX,USBRX);
-    PwmOut pwm_motor(PTA12);
-    DigitalOut motordir(PTD3);
-    float setpoint;
-    float pwm_to_motor;
-    //START OF CODE
-    pc.baud(230400);
+    pc.baud(115200);
     Ticker looptimer;
-    looptimer.attach(setlooptimerflag,0.01);  
-    pc.printf("bla");
+    looptimer.attach(setlooptimerflag,0.004);  
+    y=0;
+    y1=0;
+    y2=0;
+    z1=0;
+    z2=0;
+    
+    //Low pass, 2 Hz, 2e orde, 1 ms.
+    numl1=0.003621681514929;
+    numl2=0.007243363029857;
+    numl3=0.003621681514929;
+    //denl1=1;
+    denl2=-1.822694925196308;
+    denl3=0.837181651256023;
+    
     //INFINITE LOOP 
     while(1) {
         while(looptimerflag != true);
         looptimerflag = false;
-        setpoint = (potmeter.read()-0.5)*2000;
-        pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
-        pwm_to_motor = (setpoint - motor1.getPosition())*.001;
-        keep_in_range(&pwm_to_motor, -1,1);
-        if(pwm_to_motor > 0)
-            motordir = 0;
+        y = sluis1.read();
+        z=y*numl1+y1*numl2+y2*numl3-z1*denl2-z2*denl3;
+
+        y1=y;
+        y2=y1;
+        z1=z;
+        z2=z1;
+        
+       if(z > 0.85)
+            sluis11 = 1;
         else
-            motordir = 1;
-        //WRITE VALUE TO MOTOR  
-        pwm_motor.write(abs(pwm_to_motor));
+            sluis11 = 0;
+        //pc.printf("%f, %i \n\r", sluis10, sluis11);
+         //pc.printf("%f \n\r", z);
+         pc.printf("%i \n\r", sluis11);
+
+
     }
 }