Diff: main.cpp

Revision:

17:dc695e4e749b

Parent:

16:9e4f61aab351

Child:

18:264812048611

--- a/main.cpp	Mon Nov 03 23:19:13 2014 +0000
+++ b/main.cpp	Tue Nov 04 09:56:29 2014 +0000
@@ -241,8 +241,6 @@
     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);
-    pc.printf("setpoint : %f\r\n",setpoint);
-    pc.printf("pos_motor_1 :%f\r\n", pos_motor1_rad);
 
     if (PWM1_percentage < -100) {
         PWM1_percentage = -100;
@@ -276,18 +274,18 @@
     return out_p + out_i + out_d;
 }//float pid
 
-void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2)
+void translatie (float position2_setpoint_rad, float speed2_radpersecond)
 {
     static float setpoint = 0; 
     if (setpoint < position2_setpoint_rad) {
-        setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond);
+        setpoint += (0.005 * speed2_radpersecond);
         if (setpoint > position2_setpoint_rad) {
             setpoint = position2_setpoint_rad;
         }
     }
 
     else if (setpoint > position2_setpoint_rad) {
-        setpoint = prev_setpoint - (0.0005 * speed2_radpersecond);
+        setpoint -= (0.005 * speed2_radpersecond);
         if (setpoint < position2_setpoint_rad) {
             setpoint = position2_setpoint_rad;
         }
@@ -297,7 +295,7 @@
     }
 
     current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
-    pc.printf("c: %d\r\n", motor2.getPosition());//current_pos_motor2);
+    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);
 
@@ -353,7 +351,7 @@
             else if(current_herhalingen == 100) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                state = METEN_HOOGTE;
+                state = ARM_KALIBRATIE;
             }//else
             break;
         }//case RUST:
@@ -438,7 +436,7 @@
             }
 
 
-            translatie(position2_setpoint, 10, 0.1);
+            translatie(position2_setpoint, 5);
 
             if (current_herhalingen >= 400) {
                 current_herhalingen = 0;
@@ -463,11 +461,12 @@
 
         case RETURN2RUST: {
             GotoPosition(0,0.25);
-            translatie(0,10,0.1);
+            //translatie(0,10,0.1);
             doel_richting = 0;
             doel_hoogte = 0;
             if (current_herhalingen >= 200) {
-                translatie(0,10,0.1);
+                current_herhalingen = 0; 
+                translatie(0,0.5);
                 if (current_herhalingen >=200){
                 state = RUST;
                 current_herhalingen = 0;