Diff: main.cpp

Revision:

33:2f9c21ac9175

Parent:

32:92c8aa0408f8

Child:

34:688120048afb

--- a/main.cpp	Sat Nov 01 14:20:57 2014 +0000
+++ b/main.cpp	Sat Nov 01 15:23:43 2014 +0000
@@ -18,8 +18,8 @@
 #define WACHTEN 1
 #define SLAAN 2
 #define TERUGKEREN 3
-#define ANGLEMAX -80
-#define ANGLEMIN 0
+#define ANGLEMAX -150
+#define ANGLEMIN 150
 
 //initiating functions
 void Triceps();
@@ -80,6 +80,7 @@
 float highpass_states[4];
 
 bool stop;
+float pwm;
 float new_pos;
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje
@@ -385,18 +386,19 @@
                 }
             }
 
-            Ticker looptimer1;
+           /* Ticker looptimer1;
             //pwm_motor1.write(0.3);
             motordir1 = 1;
             stop = 0;
             looptimer1.attach(motor1aansturing,TSAMP1);
             wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer1.detach();
-            pc.printf("detachMotor1\n");
-
+            pc.printf("detachMotor1\n");*/
+            
+            toestand=TERUGKEREN;
             Ticker looptimer3;
             looptimer3.attach(motor1aansturingdeel2,TSAMP1);
-            wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
+            wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer3.detach();
             pc.printf("detachMotor1\n");
 
@@ -669,8 +671,9 @@
     if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
         toestand = TERUGKEREN;
         //motor1.setPosition(0);
-        pwm_motor1.write(0);
-        pid(0,0,true);
+        
+        pwm=pid(0,0,true);
+        pwm_motor1.write(0.0);
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
         pc.printf("toestand = terugkeren\n\r");
     }
@@ -688,7 +691,7 @@
         
     /*if (toestand == TERUGKEREN) {
         pidpositie(ANGLEMIN, motor1.getPosition());
-        }/*
+        }*/
 
 
         /*if (toestand == TERUGKEREN) {
@@ -706,14 +709,14 @@
 
 void motor1aansturingdeel2()
 {
-    if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
+    if (motor1.getPosition()>= ANGLEMIN && toestand == TERUGKEREN) {
         toestand = WACHTEN;
         //motor1.setPosition(0);
         pid(0,0,true);
         //pc.printf("if2\n");
     }
     if (toestand == TERUGKEREN) {
-        new_pwm = pid(setspeed, motor1.getSpeed(),false);
+        //new_pwm = pid(setspeed, motor1.getSpeed(),false);
         pwm_motor1.write(0.3);
         new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
         motordir1 = 0;