final

Dependencies:   Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed

Fork of Robot1_11 by BMT M9 Groep01

Revision:
29:f26796cca47f
Parent:
28:488e24ed1cb1
Child:
32:92c8aa0408f8
--- a/main.cpp	Sat Nov 01 13:12:52 2014 +0000
+++ b/main.cpp	Sat Nov 01 14:11:01 2014 +0000
@@ -18,7 +18,7 @@
 #define WACHTEN 1
 #define SLAAN 2
 #define TERUGKEREN 3
-#define ANGLEMAX -251
+#define ANGLEMAX -80
 #define ANGLEMIN 0
 
 //initiating functions
@@ -390,13 +390,13 @@
             motordir1 = 1;
             stop = 0;
             looptimer1.attach(motor1aansturing,TSAMP1);
-            wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+            wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer1.detach();
             pc.printf("detachMotor1\n");
 
             Ticker looptimer3;
             looptimer3.attach(motor1aansturingdeel2,TSAMP1);
-            wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+            wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer3.detach();
             pc.printf("detachMotor1\n");
 
@@ -670,7 +670,7 @@
         toestand = TERUGKEREN;
         //motor1.setPosition(0);
         pwm_motor1.write(0);
-        //pid(0,0,true);
+        pid(0,0,true);
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
         pc.printf("toestand = terugkeren\n\r");
     }
@@ -681,6 +681,7 @@
     }
     if (toestand == SLAAN) {
         new_pwm = pid(setspeed, motor1.getSpeed(),false);
+        new_pos=pidpositie(ANGLEMAX, motor1.getPosition());
         pwm_motor1.write(new_pwm);
         motordir1 = 1;
         pc.printf("SLAAN\n");
@@ -708,19 +709,20 @@
     if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
         //motor1.setPosition(0);
-        //pid(0,0,true);
+        pid(0,0,true);
         //pc.printf("if2\n");
     }
     if (toestand == TERUGKEREN) {
         new_pwm = pid(setspeed, motor1.getSpeed(),false);
-        pwm_motor1.write(0.2);
+        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");
     }