Diff: main.cpp

Revision:

10:48d309d9bb1c

Parent:

9:58f9e4f8229c

Child:

11:51e29f3d4545

--- a/main.cpp	Thu Oct 30 09:07:45 2014 +0000
+++ b/main.cpp	Thu Oct 30 12:21:21 2014 +0000
@@ -4,12 +4,14 @@
 
 #define TSAMP 0.001
 
+HIDScope scope(4);
 volatile bool looptimerflag;
-HIDScope scope(4);
+Ticker looptimer;
 
 void setlooptimerflag(void)
 {
     looptimerflag = true;
+    
 }
 
 void keep_in_range(float * in, float min, float max)
@@ -17,9 +19,20 @@
 *in > min ? *in < max? : *in = max: *in = max;
 }
 
-void keep_in_range(float * in, float min, float max);
+//motor 1, voltage pins op M2
+Encoder motor1(PTD3, PTD5);
+DigitalOut motor1dir(PTC9);
+PwmOut pwm_motor1(PTC8);
 
-float integral1 = 0;
+//motor 2,
+Encoder motor2(PTD2,PTD0);
+DigitalOut motor2dir(PTA4);
+PwmOut pwm_motor2(PTA5);
+
+float integral = 0;
+float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
+float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+
 float setpoint1 = 8; // te behalen speed van motor1 (37D)
 float dt1 = 0.01;
 float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
@@ -28,7 +41,6 @@
 float pwm1 = 0;
 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
 
-float integral2 = 0;
 float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
 float dt2 = 0.01;
 float Kp2 = 0.30;
@@ -39,42 +51,31 @@
 
 int main()
 {
-    wait(5);
-    //motor 1,
-    Encoder motor1(PTD3, PTD5);
-    DigitalOut motor1dir(PTC9);
-    PwmOut pwm_motor1(PTC8);
+    looptimer.attach(setlooptimerflag,TSAMP);
     pwm_motor1.period_us(100); //10kHz PWM frequency
-
-    //motor 2,
-    Encoder motor2(PTD2,PTD0);
-    DigitalOut motor2dir(PTA4);
-    PwmOut pwm_motor2(PTA5);
     pwm_motor2.period_us(100); //10kHz PWM frequency
 
-    Ticker looptimer;
-    looptimer.attach(setlooptimerflag,TSAMP);
-
-    float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
-    float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
-
+// nog een goto motor2control;
+// motor2control:
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
         //regelaar motor2, bepaalt positie
         error2 = setpoint2 - motor2.getPosition()*omrekenfactor2;
-        integral2 = integral2 + error2*TSAMP;
-        pwm2 = Kp2*error2 + Ki2*integral2;
+        integral = integral + error2*TSAMP;
+        pwm2 = Kp2*error2 + Ki2*integral;
+        
         keep_in_range(&pwm2, -1,1);
         pwm_motor2.write(abs(pwm2));
-        if(pwm2 > 0)
+        if(pwm2 > 0) {
             motor2dir = 1;
-        else
+        } else {
             motor2dir = 0;
+        }
 
         //controleert of batje positie heeft bepaald
-        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 5% voor 2 seconde, dan naar volgende motorcontrol
+        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.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
                 batjeset = 0;
             } else {
@@ -82,6 +83,8 @@
             }
         } else {
             pwm_motor2.write(0);
+            batjeset = integral = 0;
+            wait(2);
             goto motor1control;
         }
     }
@@ -93,81 +96,103 @@
 
         if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
             error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
-            integral1 = integral1 + error1*TSAMP;
-            pwm1 = Kp1*error1 + Ki1*integral1;
+            integral = integral + error1*TSAMP;
+            pwm1 = Kp1*error1 + Ki1*integral;
         } else {            //regelaar motor1, bepaalt positie
-            batjeset = integral1 = integral2 = 0;
-            goto resetposition;
+            pwm_motor1.write(0);
+            balhit = integral = 0;
+            wait(2); // wait voordat arm weer naar beginpositie terugkeert
+            goto resetpositionmotor1;
         }
 
         keep_in_range(&pwm1, -1,1);
-        pwm_motor1.write(abs(pwm1));
-
-        if(pwm1 > 0)
+        
+        if(pwm1 > 0) {
             motor1dir = 1;
-        else
+        } else {
             motor1dir = 0;
-
-        //scope set
-        scope.set(0, pwm1);
-        scope.set(1, motor1.getSpeed()*omrekenfactor1);
-        scope.set(2, motor1.getPosition()*omrekenfactor1);
-        scope.set(3, balhit);
-        scope.send();
-
+        }
+        
+        pwm_motor1.write(abs(pwm1));
+        
         //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.10) {
-            balhit = 1;
+            balhit = 1;            
         }
-
     }
-resetposition:
+    
+resetpositionmotor1:
+     while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
 
-    while(1) {      // batje en slagarm worden weer in oorspronkelijke positie geplaatst
+        //regelaar motor1, bepaalt positie
+        error1 = -1*motor1.getPosition()*omrekenfactor1;
+        integral = integral + error1*TSAMP;
+        pwm1 = Kp2*error1 + Ki2*integral;
+        
+        keep_in_range(&pwm1, -1,1);
+         
+        if(pwm1 > 0) { //HIER MOET NAAR GEKEKEN WORDEN!! de if-loop moet motor1dir op 0 zetten, maar doet dit niet, waarom?
+            motor1dir = 0;
+        } else {
+            motor1dir = 1;
+        }
+        
+        pwm_motor1.write(abs(pwm1));
+
+        //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 = 0;
+            wait(5);
+            goto resetpositionmotor2;
+        }
+    }
+    
+resetpositionmotor2:
+ while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
         //regelaar motor2, bepaalt positie
-        error2 = motor2.getPosition()*omrekenfactor2;
-        integral2 = integral2 + error2*TSAMP;
-        pwm2 = Kp2*error2 + Ki2*integral2;
+        error2 = -1*motor2.getPosition()*omrekenfactor2;
+        integral = integral + error2*TSAMP;
+        pwm2 = Kp2*error2 + Ki2*integral;
         
         keep_in_range(&pwm2, -1,1);
-        pwm_motor2.write(abs(pwm2));
-        if(pwm2 > 0)
-            motor2dir = 1;
-        else
-            motor2dir = 0;
-        
-        //regelaar motor1, bepaalt positie
-        error1 = motor1.getPosition()*omrekenfactor1;
-        integral1 = integral1 + error1*TSAMP;
-        pwm1 = Kp2*error1 + Ki2*integral1; //kp2 en ki2 worden hier gekozen (voorlopig), goede insteling voor motor2 om goed positie te bepalen, miss ook voor motor1
         
-        keep_in_range(&pwm1, -1,1);
-        pwm_motor1.write(abs(pwm1));
-        if(pwm1 > 0)
-            motor1dir = 1;
-        else
-            motor1dir = 0;
+        if(pwm2 > 0) {
+            motor2dir = 1;
+        } else {
+            motor2dir = 0;
+        }
+        
+        pwm_motor2.write(abs(pwm2));
 
-        //controleert of robot terug in oorspronkelijke positie is
-        if(batjeset < 200) {                            
-            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03 || motor1.getPosition()*omrekenfactor1 < -0.03 || motor1.getPosition()*omrekenfactor1 >0.03) {
+        //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.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
                 batjeset = 0;
             } else {
                 batjeset++;
             }
         } else {
             pwm_motor2.write(0);
-            pwm_motor2.write(0);
+            batjeset = integral = 0;
+            wait(2);
+            goto test;
             //direction = force = 0;
             //goto directionchoice;
-            goto test;            
-        }        
+        }
     }
 test:
 wait(2);
-
 }
\ No newline at end of file