Diff: main.cpp

Revision:

35:39e6e9941ce4

Parent:

34:688120048afb

Child:

36:0c8d4397c02f

--- a/main.cpp	Sat Nov 01 15:28:52 2014 +0000
+++ b/main.cpp	Sat Nov 01 16:03:45 2014 +0000
@@ -19,7 +19,7 @@
 #define SLAAN 2
 #define TERUGKEREN 3
 #define ANGLEMAX -150
-#define ANGLEMIN 150
+#define ANGLEMIN 0
 
 //initiating functions
 void Triceps();
@@ -395,8 +395,8 @@
             looptimer1.detach();
             pc.printf("detachMotor1\n");
             
-            pid(0,0,true);
-            toestand=TERUGKEREN;
+            new_pwm=pid(0,0,true);
+            
             Ticker looptimer3;
             looptimer3.attach(motor1aansturingdeel2,TSAMP1);
             wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
@@ -408,7 +408,7 @@
             myled1=1;
             myled2=1;
             myled3=1;
-        } pid(0,0,true);
+        } new_pwm=pid(0,0,true);
     } 
     //}
 }//end int main
@@ -686,7 +686,10 @@
     if (toestand == SLAAN) {
         new_pwm = pid(setspeed, motor1.getSpeed(),false);
         new_pos=pidpositie(ANGLEMAX, motor1.getPosition());
+        
         pwm_motor1.write(new_pwm);
+        //motor1.getPosition()=ANGLEMAX;
+        
         motordir1 = 1;
         pc.printf("SLAAN\n");
         
@@ -713,20 +716,21 @@
     if (motor1.getPosition()>= ANGLEMIN && toestand == TERUGKEREN) {
         toestand = WACHTEN;
         //motor1.setPosition(0);
-        pid(0,0,true);
+        new_pwm=pid(0,0,true);
         //pc.printf("if2\n");
     }
-    if (toestand == TERUGKEREN) {
+    if (toestand == TERUGKEREN && motor1.getPosition()<=ANGLEMIN) {
         //new_pwm = pid(setspeed, motor1.getSpeed(),false);
+        new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
+        //motor1.getPosition(new_pos);
         pwm_motor1.write(0.3);
-        new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
         motordir1 = 0;
 
         pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
     }
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
-        new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
+        //new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
         pc.printf("new position %f\r\n", new_pos);
         //pc.printf("ifwachten2\n");
     }