Diff: main.cpp

Revision:

13:5295c4d4be9d

Parent:

12:20ae1d20148d

--- a/main.cpp	Fri Apr 19 11:51:14 2019 +0000
+++ b/main.cpp	Sat Apr 20 14:28:59 2019 +0000
@@ -29,7 +29,7 @@
 volatile float pwm2;
 
 //Encoder
-QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
+QEI encoder1 (D1, D8, NC, 1200, QEI::X4_ENCODING);
 QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
 double Pulses1;
 double motor_position1;
@@ -50,10 +50,10 @@
 Ticker Kdc;
 
 //Servo
-Ticker ServoTick;
-DigitalOut myservo1(D8); //Duim
-DigitalOut myservo2(D11); //Pink tot Middel vinger
-DigitalOut myservo3(D10); //wijsvinger
+Ticker ServoTick;                 
+DigitalOut myservo1(D10); //Duim
+DigitalOut myservo2(D0); //Pink tot Middel vinger
+DigitalOut myservo3(D11); //wijsvinger
 
 float Periodlength = 0.02; // de MG996R heeft een PWM periode van 20 ms
 double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
@@ -773,26 +773,24 @@
                 led2 = 1;
                 led3 = 0;
 
-                servo_position1 = 0.5;
-                servo_position2 = 0.5;
-                servo_position3 = 0.5;
+                servo_position1 = Duim_krom;
+                servo_position2 = MWP_krom;
+                servo_position3 = Wijsvinger_krom;
                 ServoTick.attach(&ServoPeriod, Periodlength);
-
-                wait(1.5 );
-                servo_position1 = 0.9;
-                servo_position2 = 0.9;
-                servo_position3 = 0.9;
-                wait(1.5);
-                servo_position1 = 0.1;
-                servo_position2 = 0.1;
-                servo_position3 = 0.1;
-                wait(1);
-
+                wait(0.5 );
+                servo_position1 = Duim_recht;
+                wait(0.5 );
+                servo_position3 = Wijsvinger_recht;
+                wait(0.5);
+                servo_position2 = MWP_recht;
+                wait(0.5 );
+                servo_position1 = Duim_krom;
+                wait(0.2);
+                servo_position3 = Wijsvinger_krom;
+                wait(0.2);
+                servo_position2 = MWP_krom;
+                wait(0.2);
                 ServoTick.detach();
-
-
-                wait (1);
-
                 stateChanged = false;
             }
 
@@ -832,13 +830,13 @@
                 
                 // Servo positie
 
-                if (Dorsaal == 1 && Duim == 0) {
+                if (!But1){//(Dorsaal == 1 && Duim == 0) {
                     servo_position1 = Duim_krom;
                     servo_position2 = MWP_krom;
                     servo_position3 = Wijsvinger_krom;
                     led1 = !led1;
                 }
-                if (Dorsaal == 1 && Duim == 1) {
+                if (!But2){// (Dorsaal == 1 && Duim == 1) {
                     servo_position1 = Duim_recht;
                     servo_position2 = MWP_recht;
                     servo_position3 = Wijsvinger_recht;