Diff: main.cpp

Revision:

18:264812048611

Parent:

17:dc695e4e749b

--- a/main.cpp	Tue Nov 04 09:56:29 2014 +0000
+++ b/main.cpp	Tue Nov 04 11:28:44 2014 +0000
@@ -103,6 +103,7 @@
 float PWM_richting2;
 float PWM_richting3;
 float position2_setpoint;
+float snelheid;
 
 enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES
 state_t state = RUST;
@@ -237,7 +238,8 @@
         current_herhalingen = previous_herhalingen + 1;
         previous_herhalingen = current_herhalingen;
     }
-
+    
+    pc.printf("%f\r\n", setpoint); 
     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
     pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
     PWM1_percentage = pid(setpoint, pos_motor1_rad);
@@ -295,7 +297,6 @@
     }
 
     current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
-    pc.printf("setpoint: %f\r\n", setpoint);//current_pos_motor2);
     pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
     PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad);
 
@@ -435,8 +436,7 @@
                 position2_setpoint = 25;
             }
 
-
-            translatie(position2_setpoint, 5);
+            translatie(position2_setpoint, 1);
 
             if (current_herhalingen >= 400) {
                 current_herhalingen = 0;
@@ -448,7 +448,14 @@
         }//case INSTELLEN_RICHTING
 
         case SLAAN: {
-            GotoPosition(1.9 ,1000);
+            if (doel_hoogte == 1) {
+                snelheid = 2;
+            } else if (doel_hoogte ==2) {
+                snelheid = 2.25;
+            } else {
+                snelheid = 5;
+            }
+            GotoPosition(1.9, snelheid);
             if (current_herhalingen == 400) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
@@ -460,13 +467,12 @@
         }//case SLAAN
 
         case RETURN2RUST: {
-            GotoPosition(0,0.25);
-            //translatie(0,10,0.1);
+            GotoPosition(0,0.1);
             doel_richting = 0;
             doel_hoogte = 0;
             if (current_herhalingen >= 200) {
                 current_herhalingen = 0; 
-                translatie(0,0.5);
+                translatie(0,1.6);
                 if (current_herhalingen >=200){
                 state = RUST;
                 current_herhalingen = 0;