Diff: main.cpp

Revision:

8:aa27423f4a4a

Parent:

7:dac6b30d43b3

Child:

9:9000c5c1a0d6

--- a/main.cpp	Tue Oct 28 14:53:54 2014 +0000
+++ b/main.cpp	Wed Oct 29 16:41:46 2014 +0000
@@ -169,7 +169,7 @@
         arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
         arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
         arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
-    while(1) {
+    while(true) {
         switch(state) {
             case RUST: {                        //Aanzetten
                 lcd.cls();
@@ -269,7 +269,7 @@
                     tijdtimer.start();
                     while( tijdtimer <= 3) {
                         if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps < 0.3) { //linker goal!
-                            setpointkm = -168;        
+                            setpointkm = -125;        
                             new_pwm_km = pidkm(setpointkm, motor1.getPosition());
                             clamp(&new_pwm_km, -1,1);
                             if(new_pwm_km < 0)
@@ -277,16 +277,22 @@
                             else
                                 motordir1 = 0;
                             pwm_motor1.write(abs(new_pwm_km));
+                            if(motor1.getPosition() <= -400 ){
+                                pwm_motor1.write(0);
+                                }
                             }
                         if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
-                            setpointkm = 168;        
+                            setpointkm = 125;        
                             new_pwm_km = pidkm(setpointkm, motor1.getPosition());
                             clamp(&new_pwm_km, -1,1);
                             if(new_pwm_km < 0)
                                 motordir1 = 1;
                             else
                                 motordir1 = 0;
-                            pwm_motor1.write(abs(new_pwm_km));                            
+                            pwm_motor1.write(abs(new_pwm_km));
+                            if(motor1.getPosition() >= 400 ){
+                                pwm_motor1.write(0);
+                                }                                                        
                             }
                         if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps < 0.3) { //middelste goal!
                             setpointkm = 0;        
@@ -321,7 +327,7 @@
                 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
                 tijdtimer.start();
                 while (tijdtimer <=3 ) {
-                    if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
+                    if (kalibratiewaarde_biceps<0.3){ //kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
                         lcd.cls();
                         lcd.locate(0,0);         
                         lcd.printf("Onderste goal");   //regel 1 LCD scherm
@@ -337,6 +343,9 @@
                         else
                         motordir2 = 1;
                         pwm_motor2.write(abs(new_pwm_gm));
+                        if(motor2.getPosition() >= 450 ) {
+                        pwm_motor2.write(0); 
+                               }
                         wait(2);
                         state = RUST;
                         }
@@ -355,6 +364,8 @@
                         else
                         motordir2 = 1;
                         pwm_motor2.write(abs(new_pwm_gm));
+                        if(motor2.getPosition() >= 450 ) {
+                        pwm_motor2.write(0);                         
                         wait(2);
                         state = RUST;
                         }
@@ -373,6 +384,8 @@
                         else
                         motordir2 = 1;
                         pwm_motor2.write(abs(new_pwm_gm));
+                        if(motor2.getPosition() >= 450 ) {
+                        pwm_motor2.write(0);                         
                         wait(2);
                         state = RUST;                       
                         
@@ -381,8 +394,16 @@
                 if (tijdtimer >=3 ) {
                     tijdtimer.stop();
                     tijdtimer.reset();
-                    
+                    wait(1);
                     //+ TERUGKEREN BEGINPOSITIE!
+                    lcd.cls();
+                    lcd.locate(0,0);         
+                    lcd.printf("Goed Gedaan!");   //regel 1 LCD scherm
+                    lcd.locate(0,1);
+                    lcd.printf("Terug naar begin");             //regel 2 LCD scherm
+                    setpointgm = (0);
+                    new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
+                    clamp(&new_pwm_gm, -1,1);
                     state = RUST;
                 } //einde if statement