Diff: main.cpp

Revision:

27:691779624530

Parent:

26:9859a71456fd

Child:

28:c33a0658605e

--- a/main.cpp	Sat Nov 01 12:52:52 2014 +0000
+++ b/main.cpp	Sat Nov 01 13:43:21 2014 +0000
@@ -71,7 +71,7 @@
 float lowpass_deltoid_states[4];
 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
-//highpass filter settings: Fc = 10 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 49-51, Fs =500Hz, Gain = -3 dB (van Melvin)
+//highpass filter settings: Fc = 10 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
 float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
 //state values
 float highnotch_biceps_states[8];
@@ -403,7 +403,7 @@
 
 void arm_begin ()
 {
-    speed2_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
+    speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
     if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
         setpoint2 = (0.0*2.0*PI/360);
@@ -460,37 +460,38 @@
             wait_iterator1 = 0;
         } else if(staat1 ==1) {
             wait_iterator1++;
-            if(wait_iterator1 > 1000)
+            if(wait_iterator1 > 400) {
                 staat1 = 2;
-        } else {
-            batje_begin_rechts();
+            
+                batje_begin_rechts();
+            }
         }
     }
-
     if (batje_hoek == 2) {
         if(staat1 == 0) {
             batje_links();
             wait_iterator1 = 0;
         } else if(staat1 ==1) {
             wait_iterator1++;
-            if(wait_iterator1 > 1000)
+            if(wait_iterator1 > 400) {
                 staat1 = 2;
-        } else {
-            batje_begin_links ();
+           
+                batje_begin_links ();
+            }
         }
     }
 
-
     if(arm_hoogte == 1) {
         if(staat2 == 0) {
             arm_laag();
             wait_iterator2 = 0;
         } else if(staat2 == 1) {
             wait_iterator2++;
-            if(wait_iterator2 > 400)
+            if(wait_iterator2 > 400) {
                 staat2 = 2;
-        } else {
-            arm_begin();
+            
+                arm_begin();
+            }
         }
     }
     if(arm_hoogte == 2) {
@@ -499,10 +500,11 @@
             wait_iterator2 = 0;
         } else if(staat2 == 1) {
             wait_iterator2++;
-            if(wait_iterator2 > 400)
+            if(wait_iterator2 > 400) {
                 staat2 = 2;
-        } else {
-            arm_begin();
+           
+                arm_begin();
+            }
         }
     }
     if(arm_hoogte == 3) {
@@ -511,14 +513,17 @@
             wait_iterator2 = 0;
         } else if(staat2 == 1) {
             wait_iterator2++;
-            if(wait_iterator2 > 400)
+            if(wait_iterator2 > 400) {
                 staat2 = 2;
-        } else {
-            arm_begin();
+            
+                arm_begin();
+            }
         }
     }
+
 }
 
+
 int main()
 {