Diff: main.cpp

Revision:

36:0c8d4397c02f

Parent:

35:39e6e9941ce4

Child:

37:35fda673beb3

--- a/main.cpp	Sat Nov 01 16:03:45 2014 +0000
+++ b/main.cpp	Sat Nov 01 23:23:49 2014 +0000
@@ -100,6 +100,11 @@
 DigitalOut myled2(LED2);//green
 DigitalOut myled3(LED3);//blue
 
+DigitalOut rood(PTD1);
+DigitalOut groen(PTA13);
+DigitalOut blauw(PTA12);
+DigitalOut wit(PTD4);
+
 /* FRDM-KL25Z built-in touch slider
 *******************
 *     *     *     *
@@ -144,6 +149,7 @@
             myled1 = 0;
             myled2 = 1;
             myled3 = 1;
+            rood=1;
 
             /*pc.printf("calibratie tricep aan\n");
             wait(2);
@@ -156,17 +162,22 @@
             myled1 = 0;
             myled2 = 0;
             myled3 = 0;
+            wit=1;
+
             wait(2);
 
             myled1=1;
             myled2=1;
-            myled3=1;*/
+            myled3=1;
+            rood=0;
+            wit=0;*/
         }
         if (key==2) {
             //green
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
+            groen=1;
 
             /*pc.printf("calibratie bicep snelheid 1 aan\n");
             wait(2);
@@ -177,8 +188,10 @@
             myled1 = 0;
             myled2 = 0;
             myled3 = 0;
+            wit=1;
             wait(2);
 
+            wit=0;
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
@@ -192,8 +205,11 @@
             myled1 = 0;
             myled2 = 0;
             myled3 = 0;
+            wit=1;
+
             wait(2);
 
+            wit=0;
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
@@ -206,9 +222,13 @@
             myled1 = 0;
             myled2 = 0;
             myled3 = 0;
+            wit=1;
+
             wait(2);
 
             pc.printf("caliratie biceps is klaar\n");
+            wit=0;
+            groen=0;
             myled1=1;
             myled2=1;
             myled3=1;*/
@@ -219,6 +239,7 @@
             myled1 = 1;
             myled2 = 1;
             myled3 = 0;
+            blauw=1;
             /*wait(3);
 
             if(drempelwaardeT==0) {
@@ -226,24 +247,32 @@
                 myled1 = 0;
                 myled2 = 0;
                 myled3 = 0;
+                wit=1;
+                rood=1;
             }
             if (drempelwaardeB1==0) {
                 pc.printf("geen waarde calibratie BICEPS 1 \n");
                 myled1 = 0;
                 myled2 = 0;
                 myled3 = 0;
+                wit=1;
+                groen=1;
             }
             if (drempelwaardeB2==0) {
                 pc.printf("geen waarde calibratie BICEPS 2 \n");
                 myled1 = 0;
                 myled2 = 0;
                 myled3 = 0;
+                wit=1;
+                groen=1;
             }
             if (drempelwaardeB3==0) {
                 pc.printf("geen waarde calibratie BICEPS 3 \n");
                 myled1 = 0;
                 myled2 = 0;
                 myled3 = 0;
+                wit=1;
+                groen=1;
             } else {*/
 
             /*pc.printf("eerst positie dan snelheid aangeven /n");
@@ -258,6 +287,7 @@
             myled1 = 0;
             myled2 = 1;
             myled3 = 1;
+            rood=1;
 
             log_timerT1.attach(Triceps, 0.005);
             wait(2);
@@ -275,6 +305,8 @@
             myled1 = 1;
             myled2 = 1;
             myled3 = 0;
+            rood=0;
+
             wait(3);
 
             //bepaling van positie met tricep 2
@@ -287,6 +319,7 @@
             myled1 = 0;
             myled2 = 1;
             myled3 = 1;
+            rood=1;
 
             log_timerT2.attach(Triceps, 0.005);
             wait(2);
@@ -302,6 +335,7 @@
             myled1 = 1;
             myled2 = 1;
             myled3 = 0;
+            rood=0;
 
             //*** INPUT MOTOR 2 ***
             positie=yT1+yT2;
@@ -336,6 +370,7 @@
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
+            groen=1;
 
             log_timerB.attach(Biceps,0.005);
             wait(2);
@@ -363,6 +398,7 @@
             myled1 = 1;
             myled2 = 1;
             myled3 = 0;
+            groen=0;
 
             //*** INPUT MOTOR 1 ***
             snelheidsstand=yB1+yB2+yB3; */
@@ -394,9 +430,9 @@
             wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer1.detach();
             pc.printf("detachMotor1\n");
-            
-            new_pwm=pid(0,0,true);
-            
+
+            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,8 +444,12 @@
             myled1=1;
             myled2=1;
             myled3=1;
-        } new_pwm=pid(0,0,true);
-    } 
+            groen=0;
+            blauw=0;
+
+            pid(0,0,true);
+        }
+    }
     //}
 }//end int main
 
@@ -648,7 +688,6 @@
 
 void motor1aansturing()
 {
-
     if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
         pc.printf("if1\n");
@@ -672,30 +711,29 @@
     if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
         toestand = TERUGKEREN;
         //motor1.setPosition(0);
-        
-        pwm=pid(0,0,true);
-        pwm_motor1.write(0.0);
+        pid(0,0,true);
+        pwm_motor1.write(0);//arvid had hier 0,0 gezet?!
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
         pc.printf("toestand = terugkeren\n\r");
     }
     if (toestand == WACHTEN) {
-        pidpositie(ANGLEMAX, motor1.getPosition());
+        pidpositie(ANGLEMIN,motor1.getPosition()); //dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
+        //?? motor1.getPosition(nieuwe positie);
         pwm_motor1.write(0);
         pc.printf("ifwachten\n");
     }
     if (toestand == SLAAN) {
+        pidpositie(ANGLEMAX, motor1.getPosition());//dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
+        //?? motor1.getPosition(nieuwe positie);
         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");
-        
-    /*if (toestand == TERUGKEREN) {
-        pidpositie(ANGLEMIN, motor1.getPosition());
-        }*/
+
+        /*if (toestand == TERUGKEREN) {
+            pidpositie(ANGLEMIN, motor1.getPosition());
+            }*/
 
 
         /*if (toestand == TERUGKEREN) {
@@ -705,24 +743,25 @@
             pc.printf("motor gaat terugkeren\n\r");
             pc.printf("new pwm %f\r\n",new_pwm);*/
     }
-    scope.set(0,motor1.getPosition());        //ruwe data
-    scope.set(1,motor1.getPosition());     //filtered
+    scope.set(0,motor1.getPosition());
+    scope.set(1,motor1.getPosition());
     scope.send();
 
 }
 
 void motor1aansturingdeel2()
 {
-    if (motor1.getPosition()>= ANGLEMIN && toestand == TERUGKEREN) {
+    if (toestand == TERUGKEREN && motor1.getPosition()>= ANGLEMIN) {//moet arm hier niet naar beginpositie? als het te ver naar achteren is gegaan bijvoorbeeld
+        //pidpositie(ANGLEMIN, motor1.getPosition())
+        //?? motor1.getPosition(nieuwe positie);
         toestand = WACHTEN;
         //motor1.setPosition(0);
-        new_pwm=pid(0,0,true);
+        pid(0,0,true);
         //pc.printf("if2\n");
     }
     if (toestand == TERUGKEREN && motor1.getPosition()<=ANGLEMIN) {
-        //new_pwm = pid(setspeed, motor1.getSpeed(),false);
-        new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
-        //motor1.getPosition(new_pos);
+        pidpositie(ANGLEMIN, motor1.getPosition());//dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
+        //?? motor1.getPosition(nieuwe positie);
         pwm_motor1.write(0.3);
         motordir1 = 0;
 
@@ -730,7 +769,7 @@
     }
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
-        //new_pos=pidpositie(ANGLEMIN, motor1.getPosition());
+        //pidpositie(ANGLEMIN, motor1.getPosition());
         pc.printf("new position %f\r\n", new_pos);
         //pc.printf("ifwachten2\n");
     }