Diff: main.cpp

Revision:

8:5dab7ea40bc1

Parent:

7:59116528d895

Child:

9:58f9e4f8229c

diff -r 59116528d895 -r 5dab7ea40bc1 main.cpp
--- a/main.cpp	Tue Oct 28 13:49:24 2014 +0000
+++ b/main.cpp	Wed Oct 29 15:07:47 2014 +0000
@@ -2,109 +2,124 @@
 #include "encoder.h"
 #include "HIDScope.h"
 
+#define TSAMP 0.001
 
-HIDScope scope(6);
+volatile bool looptimerflag;
+HIDScope scope(4);
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+void keep_in_range(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = max;
+}
 
 void keep_in_range(float * in, float min, float max);
 
+float integral1 = 0;
+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
+float Ki1 = 0.20;
+float error1 = 0;
+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;
+float Ki2 = 0.20;
+float error2 = 0;
+float pwm2 = 0;
+float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);
+
 int main()
 {
-    //motor 1, 25D
+    wait(5);
+    //motor 1,
     Encoder motor1(PTD3, PTD5);
     DigitalOut motor1dir(PTC9);
     PwmOut pwm_motor1(PTC8);
     pwm_motor1.period_us(100); //10kHz PWM frequency
 
-    //motor 2, 25D
+    //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 integral1 = 0;
-    float setpoint1 = 8;
-    float dt1 = 0.001;
-    float Kp1 = 1.5;
-    float Ki1 = 0.5;
-    float error1 = 0;
-    float output1 = 0;
-    float omrekenfactor1 = (32*70)/6.28;
+    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
+
+    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
 
-    float integral2 = 0;
-    float setpoint2 = 3.14;
-    float dt2 = 0.001;
-    float Kp2 = 0.55;
-    float Ki2 = 0.45;
-    float error2 = 0;
-    float output2 = 0;
-    float omrekenfactor2 = (24*172)/6.28;
-
-    float setpointrem1 = 0;
-    float integralrem1 = 0;
-    float Kprem1 = 1.5;
-    float Kirem1 = 0.5;
-    float errorrem1 = 0;
-    float outputrem1 = 0;
+        //regelaar motor2, bepaalt positie
+        error2 = setpoint2 - motor2.getPosition()*omrekenfactor2;
+        integral2 = integral2 + error2*TSAMP;
+        pwm2 = Kp2*error2 + Ki2*integral2;
+        keep_in_range(&pwm2, -1,1);
+        pwm_motor2.write(abs(pwm2));
+        if(pwm2 > 0)
+            motor2dir = 1;
+        else
+            motor2dir = 0;
 
-    
-    while(1) {
-        error2 = setpoint2 - motor2.getPosition()/omrekenfactor2;
-        integral2 = integral2+ error2*dt2;
-        output2 = Kp2*error2 + Ki2*integral2;
-        keep_in_range(&output2,-1,1);
-        pwm_motor2.write(abs(output2));
-        if(output2 > 0) {
-            motor2dir = 1;
+        //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 (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
+                batjeset = 0;
+            } else {
+                batjeset++;
+            }
         } else {
-            motor2dir = 0;
+            pwm_motor2.write(0);
+            goto motor1control;
         }
-        wait(dt2);
+    }
 
-        error1 = setpoint1 - motor1.getSpeed()/omrekenfactor1;
-        //motorpositie omgerekend naar rad/s
-        integral1 = integral1 + error1*dt1;
-        output1 = Kp1*error1 + Ki1*integral1;
-        keep_in_range(&output1,-1,1);
-        pwm_motor1.write(abs(output1));
-        if(output1 > 0) {
-            motor1dir = 1;
+motor1control:
+    while(1) {      // loop voor het goed plaatsen van motor1 (batje snelheid)
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+
+        //regelaar motor1, bepaalt snelheid
+        if (balhit == 0) {
+            error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+            integral1 = integral1 + error1*TSAMP;
+            pwm1 = Kp1*error1 + Ki1*integral1;
+            keep_in_range(&pwm1, -1,1);
+            pwm_motor1.write(abs(pwm1));
         } else {
-            motor1dir = 0;
-        }
-        wait(dt1);
-
-        if(motor1.getPosition()/omrekenfactor1 < 1.3) {
-            setpoint1=8;
+            pwm_motor1.write(0);
+            goto motorcontroldone;
         }
 
-        else {
-            setpoint1=0;
+        if(pwm1 > 0)
+            motor1dir = 1;
+        else
+            motor1dir = 0;
 
-            /*errorrem1 = setpointrem1 - motor1.getSpeed()/omrekenfactor1;
-            //motorpositie omgerekend naar rad/s
-            integralrem1 = integralrem1 + errorrem1*dt1;
-            outputrem1 = Kprem1*errorrem1 + Kirem1*integralrem1;
-            keep_in_range(&outputrem1,-1,1);
-            pwm_motor1.write(abs(outputrem1));
-            if(outputrem1 > 0) {
-                motor1dir = 1;
-            } else {
-                motor1dir = 0;
-            }
-            wait(dt1);*/
-        }
+        //scope set
+        scope.set(0, pwm1);
+        scope.set(1, motor1.getSpeed()*omrekenfactor1);
+        scope.set(2, motor1.getPosition()*omrekenfactor1);
+        scope.set(3, balhit);
+        scope.send();
 
-        scope.set(0, output1);
-        scope.set(1, motor1.getSpeed()/omrekenfactor1);
-        scope.set(2, motor1.getPosition()/(omrekenfactor1));
-        scope.set(3, output2);
-        scope.set(4, motor2.getSpeed()/omrekenfactor2);
-        scope.set(5, motor2.getPosition()/(omrekenfactor2));
-        scope.send();
+        //controleert of batje balletje heeft bereikt
+        if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) {
+            balhit = 1;
+        }
     }
-}
-void keep_in_range(float * in, float min, float max)
-{
-*in > min ? *in < max? : *in = max: *in = max;
+motorcontroldone:
+    wait(1);
 }
\ No newline at end of file