Diff: PROJECT_main.cpp

Revision:

25:1ec0da9b26a5

Parent:

24:1b812b393264

Child:

26:8d91c4c54bb6

--- a/PROJECT_main.cpp	Tue Nov 04 10:39:45 2014 +0000
+++ b/PROJECT_main.cpp	Tue Nov 04 11:28:21 2014 +0000
@@ -543,20 +543,19 @@
             scope.set(4, state);
 
             switch(state) {
-                case 1: {
-                    setpoint1=0;
-                    //setpoint2 += 1*TSAMP;
+                case 1: 
                     switch (direction) {
                         case 1:
                             pwm_motor2.write(0.8);
                             motor2dir = 1;
                             count++;
-                            if (count>140) {
+                            if (count>170) { 
                                 state=2;
                                 count = 0;
                                 pwm_motor2.write(0);
                             }
                             break;
+                
                         case 2:
                             pwm_motor2.write(0.8);
                             motor2dir = 1;
@@ -589,27 +588,27 @@
                 case 3: {
                     switch (force) {
                         case 1:
-                            pwm_motor1.write(0.8);
-                            motor2dir = 1;
+                            pwm_motor1.write(1);
+                            motor1dir = 1;
                             break;
                         case 2:
-                            pwm_motor1.write(0.8);
-                            motor2dir = 1;
+                            pwm_motor1.write(1); //net niet haalbaar
+                            motor1dir = 1;
                             break;
                         case 3:
-                            pwm_motor1.write(0.8);
-                            motor2dir = 1;
+                            pwm_motor1.write(1); //niet haalbaar
+                            motor1dir = 1;
                             break;
                     }
-                    if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
-                        setpoint1 = 2.36; 
+                    if(fabs(motor1.getPosition()*omrekenfactor1)>7.0) {
+                        pwm_motor1.write(0); 
                         state = 4;
                     }
                     break;
                 }
                 case 4: {
                     count++;
-                    if(count>1000) {
+                    if(count>4000) {
                         count = 0;
                         state = 1;
                         goto motor2cal;
@@ -617,163 +616,5 @@
                     break;
                 }                
             }
-
-            //motor regeling
-            
-            /*
-            //regelaar motor1, bepaalt positie
-            controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
-            integral1 = integral1 + controlerror1*TSAMP;
-            derivative1 = (controlerror1 - previouserror1)/TSAMP;
-            pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
-            previouserror1 = controlerror1;
-            scope.set(5, pwm1);
-            
-            keep_in_range(&pwm1, -1,1);
-            pwm_motor1.write(fabs(pwm1));
-            if(pwm1 > 0) {
-                motor1dir = 1;
-            } else {
-                motor1dir = 0;
-            }
-            */
-/*
-            //regelaar motor2, bepaalt positie
-            controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
-            integral = integral + controlerror*TSAMP;
-            derivative = (controlerror - previouserror)/TSAMP;
-            pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
-            previouserror = controlerror;
-
-            keep_in_range(&pwm, -1,1);
-            pwm_motor2.write(fabs(pwm));
-            if(pwm > 0) {
-                motor2dir = 1;
-            } else {
-                motor2dir = 0;
-            }
-*/
-            /*
-                    //controleert of batje positie heeft bepaald
-                    if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
-                        if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
-                            batjeset = 0;
-                        } else {
-                            batjeset++;
-                        }
-                    } else {
-                        pwm_motor2.write(0);
-                        batjeset = integral = derivative = previouserror = 0;
-                        wait(1);
-                        //goto motor1control;
-                    }
-            */
-        }
-        /*
-        motor1control:
-            while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
-                while(!looptimerflag);
-                looptimerflag = false; //clear flag
-
-                if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
-                    controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
-                    integral = integral + controlerror*TSAMP;
-                    derivative = (controlerror - previouserror)/TSAMP;
-                    pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
-                    previouserror = controlerror;
-                } else {            //regelaar motor1, bepaalt positie
-                    balhit = integral = derivative = previouserror = 0;
-                    goto resetpositionmotor1;
-                }
-
-                keep_in_range(&pwm, -1,1);
-                pwm_motor1.write(abs(pwm));
-
-                if(pwm > 0) {
-                    motor1dir = 1;
-                } else {
-                    motor1dir = 0;
-                }
-
-                //controleert of batje balletje heeft bereikt
-                //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
-                if (motor1.getPosition()*omrekenfactor1 > 1.60) {
-                    balhit = 1;
-                }
-            }
-        // FORMAT_CODE_END
-
-        resetpositionmotor1:
-            while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
-                while(!looptimerflag);
-                looptimerflag = false; //clear flag
-
-                //regelaar motor1, bepaalt positie
-                controlerror = -1*motor1.getPosition()*omrekenfactor1;
-                integral = integral + controlerror*TSAMP;
-                derivative = (controlerror - previouserror)/TSAMP;
-                pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
-                previouserror = controlerror;
-
-                keep_in_range(&pwm, -1,1);
-                if(pwm > 0) {
-                    motor1dir = 1;
-                } else {
-                    motor1dir = 0; //1 = rechtsom, 0 = linksom
-                }
-
-                pwm_motor1.write(abs(pwm));
-
-                //controleert of arm terug in positie is
-                if(batjeset < 200) {
-                    if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
-                        batjeset = 0;
-                    } else {
-                        batjeset++;
-                    }
-                } else {
-                    pwm_motor1.write(0);
-                    batjeset = integral = derivative = previouserror = 0;
-                    wait(1);
-                    goto resetpositionmotor2;
-                }
-            }
-
-        resetpositionmotor2:
-            while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
-                while(!looptimerflag);
-                looptimerflag = false; //clear flag
-
-                //regelaar motor2, bepaalt positie
-                controlerror = -1*motor2.getPosition()*omrekenfactor2;
-                integral = integral + controlerror*TSAMP;
-                derivative = (controlerror - previouserror)/TSAMP;
-                pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
-                previouserror = controlerror;
-
-                keep_in_range(&pwm, -1,1);
-
-                if(pwm > 0) {
-                    motor2dir = 1;
-                } else {
-                    motor2dir = 0;
-                }
-
-                pwm_motor2.write(abs(pwm));
-
-                //controleert of batje positie heeft bepaald
-                if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
-                    if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
-                        batjeset = 0;
-                    } else {
-                        batjeset++;
-                    }
-                } else {
-                    pwm_motor2.write(0);
-                    batjeset = integral = derivative = previouserror = 0;
-                    wait(1);
-                    direction = force = 0;
-                    goto motor1cal;
-                }
-            }*/
+        }        
     } // end main
\ No newline at end of file