robot

Dependencies:   Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed

Fork of Robot2false by BMT M9 Groep01

Revision:
63:90cc947c3d77
Parent:
62:bdcc3328b13e
Child:
64:21e3e832f127
--- a/main.cpp	Tue Nov 04 07:43:10 2014 +0000
+++ b/main.cpp	Tue Nov 04 08:48:21 2014 +0000
@@ -85,7 +85,7 @@
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje
 int toestand = WACHTEN;
-float setspeed = 0, V3=150, V2=40, V1 =30;//V in counts/s
+float setspeed = 0, V3=150, V2=1, V1 =10;//V in counts/s
 
 Encoder motor1(PTD5,PTD3, true);
 Encoder motor2(PTD0,PTD2);
@@ -142,7 +142,7 @@
     pc.printf("key 3 START\r\n");
 
     while(true) {
-        
+
         key = TButton.PressedButton();
 
         if (key==1) {
@@ -240,7 +240,7 @@
             myled2 = 1;
             myled3 = 0;
             blauw=1;
-            wait(3);
+            /*wait(3);
 
             if(drempelwaardeT==0) {
                 pc.printf("geen waarde calibratie TRICEPS \n");
@@ -274,7 +274,7 @@
                 wit=1;
                 groen=1;
             } else {
-                pc.printf("eerst positie dan snelheid aangeven /n");
+                pc.printf("eerst positie dan snelheid aangeven /n"); */
 
                 //bepaling van positie met triceps 1
                 /*Ticker log_timerT1;
@@ -337,7 +337,7 @@
                 rood=0;
 
                 // *** INPUT MOTOR 2 ***
-                positie=yT1+yT2; */
+                positie=yT1+yT2;
 
                 //controle positie op scherm
                 if (positie==0) {
@@ -358,7 +358,7 @@
                 looptimer2.detach();
                 pc.printf("Detach Motor 2\n");
 
-                //   eind aansturing motor 2
+                //   eind aansturing motor 2*/
 
                 wait(2);
                 /*Ticker log_timerB;
@@ -402,8 +402,8 @@
 
                 // *** INPUT MOTOR 1 ***
                 snelheidsstand=yB1+yB2+yB3; */
-
-                //controle snelheidsstand op scherm
+                snelheidsstand=2;
+                               //controle snelheidsstand op scherm
                 if (snelheidsstand==0) {
                     pc.printf("Motor 1 beweegt niet \n");
                 } else {
@@ -445,7 +445,7 @@
                 groen=0;
                 blauw=0;
             }
-        }
+        //}
     }
 }//end int main
 
@@ -697,7 +697,8 @@
         case SLAAN:
             pc.printf("SLAAN\n");
             new_pwm = pid(setspeed, omega);
-            pwm_motor1.write(1);
+            pwm_motor1.write(new_pwm);
+            pc.printf("new-Pwm %f/r/n", new_pwm);
             motordir1 = 1;
             if (motor1.getPosition() <= ANGLEMAX) {
                 toestand = TERUGKEREN;
@@ -745,7 +746,7 @@
     omega=(deltacounts/TSAMP1);
 
     switch(toestand) {
-        case TERUGKEREN: 
+        case TERUGKEREN:
             new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());
             if (new_pwmpos > 0) {
                 motordir1 = 0;
@@ -753,11 +754,11 @@
             } else {
                 motordir1 = 1;
             }
-            pwm_motor1.write(fabs(new_pwmpos)); 
+            pwm_motor1.write(fabs(new_pwmpos));
             break;
     } //end switch
 
-    scope.set(0,motor1.getPosition());        
+    scope.set(0,motor1.getPosition());
     scope.set(2,ANGLEMIN);
     scope.set(3, omega);
     scope.set(4, setspeed);