Diff: main.cpp

Revision:

2:f3e8a27d376c

Parent:

1:a010e434a360

Child:

3:611fd72c9d46

--- a/main.cpp	Fri Oct 31 12:03:55 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:50:14 2014 +0000
@@ -11,10 +11,14 @@
 
 // Outputs van de motor
 Encoder encoderA(PTD5,PTA13);
+Encoder encoder1(PTD0,PTD2);
+
 
 // Outputs naar de motor
 PwmOut pwm(PTC8);
 DigitalOut dir(PTC9);
+PwmOut pwm1(PTA5);
+DigitalOut dir1(PTA4);
 
 // Vaststellen van het datatype van de outputs naar de motor
 float s_speed1;
@@ -85,7 +89,8 @@
 int armstand=0;
 int armspeed=0;
 bool speeddone=0;
-
+int a; //wordt gebruikt in gotopos
+float out1; //pid voor armpositie
 // Teller voor hoeveel metingen er zijn gedaan
 uint16_t teller=0;
 
@@ -286,7 +291,6 @@
         badjes=1;
         check=0;
     }
-
     else {
         check=0;
         badjes = 2;
@@ -307,18 +311,19 @@
         check=1;
     } else if (y1>0) {
         if (speed>0) {
-            speed=speed-1;
+            speed+=1;
         } else {
             check=0;
         }
     } else if (y2>0 ) {
         if (speed<2) {
-            speed=speed+1;
+            speed-=1;
         } else {
             check=0;
         }
     } else {
         check=0;
+        
     }
     return speed;
 }
@@ -349,9 +354,104 @@
 
 }
 
+void clamp(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+float pid1(float setpoint, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+
+    float           out_d = 0;
+    error  = setpoint-measurement;
+    out_p  = error*K_P;
+    out_i += error*K_I;
+    out_d  = (error-prev_error)*K_D;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    prev_error = error;
+    out1  = out_i+out_p+out_d;
+    return out1;
+}
+
+float getv(float delta_t)
+{
+    int n =0 ;
+    while(n<3) {
+        wait(delta_t);
+        enc = encoderA.getPosition();
+        enca2 = enca1;
+        enca1 = enc;
+        n++;
+
+    }
+
+    counts = (enca1 - enca2)/delta_t;
+    v = (counts)*((2*3.14159265359)/1550);
+    return v;
+}
+
+
+float gotopos(float pos)
+{
+    switch(a) {
+        case 1:
+            pos = 100;
+            break;
+
+        case 2:
+            pos = 200;
+            break;
+
+        case 3:
+            pos = 250;
+            break;
+    }
+    enc = encoder1.getPosition();
+    while((abs(pos-encoder1.getPosition()) >6)|| (v!= 0)) {
+
+        while(!looptimerflag);
+        looptimerflag = false;
+        out1 = pid1(pos,encoder1.getPosition());
+
+        if(out>0) {
+            dir1 = 1;
+
+        } else if(out<0) {
+            dir1 = 0;
+        }
+        pwm1 = fabs(out);
+        v = getv(0.001);
+    }
+    pwm1 =0;
+
+    return pwm1;
+}
+
+float reset()
+{
+    v = 1;
+    while(v !=0) {
+
+        dir = 0;
+        speed = 0.1;
+        pwm = speed;
+        v =getv(0.1);
+    }
+    pwm = 0;
+    dir =1;
+    encoderA.setPosition(0);
+    zero = encoderA.getPosition();
+
+    return pwm;
+}
+
 // Main code
 int main()
 {
+     v = 1;
     rood = 0;
     blauw = 1;
     groen = 1;
@@ -397,7 +497,7 @@
             } else if (badjedone==1) {
                 // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt
                 if (speeddone==1) {
-                    armspeed=armstand+1;
+                    
                     badjedone=0;
                     speeddone=0;
                     badjestand=1;
@@ -405,6 +505,7 @@
                     // Anders, pas de hoek van de arm aan naar de stand die hoort bij de huidig geselecteerde snelheid
                 } else {
                     armstand=velocity(y1,y2);
+                    gotopos(armstand);
                 }
 
             }
@@ -412,6 +513,8 @@
         // Verzend data naar de scopes
 
         viewer();
+        reset();
+        
 
     }