Diff: PROJECT_main.cpp

Revision:

24:1b812b393264

Parent:

23:913bf8a6f7d8

Child:

25:1ec0da9b26a5

--- a/PROJECT_main.cpp	Tue Nov 04 10:35:26 2014 +0000
+++ b/PROJECT_main.cpp	Tue Nov 04 10:39:45 2014 +0000
@@ -589,13 +589,16 @@
                 case 3: {
                     switch (force) {
                         case 1:
-                            setpoint1 += 2.5*TSAMP; //6.8*TSAMP;
+                            pwm_motor1.write(0.8);
+                            motor2dir = 1;
                             break;
                         case 2:
-                            setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
+                            pwm_motor1.write(0.8);
+                            motor2dir = 1;
                             break;
                         case 3:
-                            setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
+                            pwm_motor1.write(0.8);
+                            motor2dir = 1;
                             break;
                     }
                     if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
@@ -605,29 +608,14 @@
                     break;
                 }
                 case 4: {
-                    setpoint2 -= 0.25*TSAMP;
-                    if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm
-                        state = 5;
-                    }
-                    break;
-                }
-                case 5: {
-                    setpoint1 -= 0.5*TSAMP;
-                    if(setpoint1 < 0) {
-                        state = 6;
-                    }
-                    break;
-                }
-                case 6: {
-                    setpoint1 = 0;
                     count++;
-                    if(count>3000) {
+                    if(count>1000) {
                         count = 0;
                         state = 1;
                         goto motor2cal;
                     }
                     break;
-                }
+                }                
             }
 
             //motor regeling