Diff: main.cpp

Revision:

3:2aaf54ce090b

Parent:

2:7c9974f0947a

Child:

4:babe09a69296

diff -r 7c9974f0947a -r 2aaf54ce090b main.cpp
--- a/main.cpp	Wed Mar 27 17:33:56 2019 +0000
+++ b/main.cpp	Thu Apr 04 15:37:37 2019 +0000
@@ -3,7 +3,7 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "BiQuad.h"
-
+#include "FastPWM.h"
 // Algemeen
 DigitalIn button3(SW3);  
 DigitalIn button2(SW2); 
@@ -19,8 +19,8 @@
 Timer t2;
 //Motoren
 DigitalOut direction1(D4);
-PwmOut pwmpin1(D5);
-PwmOut pwmpin2(D6);
+FastPWM pwmpin1(D5);
+FastPWM pwmpin2(D6);
 DigitalOut direction2(D7);
 volatile float pwm1;
 volatile float pwm2;
@@ -33,6 +33,7 @@
 double Pulses2;
 double motor_position2;
 double error1;
+double u1;
 
 //Pot meter
 AnalogIn pot(A1);
@@ -45,6 +46,15 @@
 Ticker PotRead;
 Ticker Kdc;
 
+// EMG
+float EMG1;       // Rotatie
+float EMG2;       // Elleboog
+float EMG3;       // Hand
+float EMG4;       // Reverse
+float Input1;       // Voor zonder EMG
+float Input2;
+int count = 0;
+
 //Kinematica
 double stap1;
 double stap2;
@@ -66,32 +76,32 @@
 float Hoeknieuw2;
 
 //Limiet in graden
-float lowerlim1 = 0;
-float upperlim1 = 748.8;
-float lowerlim2 = 0;
-float upperlim2 = 1300;   
+float lowerlim1 = -900;
+float upperlim1 = 900;
+float lowerlim2 = -50;
+float upperlim2 = 1500;   
 
 // VARIABLES PID CONTROLLER
-double Kp1 = 5;          
+double Kp1 = 12.5;          
 double Ki1 = 0;          
 double Kd1 = 1;
-double Kp2 = 6;          // Zonder arm: 6,0,1
+double Kp2 = 12;          // Zonder arm: 6,0,1, met rotatie: 10, 0, 1
 double Ki2 = 0;          
 double Kd2 = 1;          
 double Ts = 0.0005;      // Sample time in seconds
 
 // Functies Kinematica
-float Kinematics1(float KPot)
+float Kinematics1(float EMG1)
 {
 
-    if (KPot > 0.45f) {
-        stap1 = KPot*150*Ts;
+    if (EMG1 > 0.45f) {
+        stap1 = EMG1*450*Ts;
         Hoeknieuw1 = PolsReference + stap1;
         return Hoeknieuw1;
     }
 
-    else if (KPot < -0.45f) {
-        stap1 = KPot*150*Ts;
+    else if (EMG1 < -0.45f) {
+        stap1 = EMG1*450*Ts;
         Hoeknieuw1 = PolsReference + stap1;
         return Hoeknieuw1;
     }
@@ -100,17 +110,17 @@
         return PolsReference;
     }
 }
-float Kinematics2(float KPot)
+float Kinematics2(float EMG2)
 {
 
-    if (KPot > 0.45f) {
-        stap2 = KPot*300*Ts;         
+    if (EMG2 > 0.45f) {
+        stap2 = EMG2*300*Ts;         
         Hoeknieuw2 = ElbowReference + stap2;
         return Hoeknieuw2;
     }
 
-    else if (KPot < -0.45f) {
-        stap2 = KPot*300*Ts;
+    else if (EMG2 < -0.45f) {
+        stap2 = EMG2*300*Ts;
         Hoeknieuw2 = ElbowReference + stap2;
         return Hoeknieuw2;
     }
@@ -208,12 +218,12 @@
 void moter1_control(double u1)
 {
     direction1= u1 > 0.0f; //positief = CW
-    if (fabs(u1)> 0.5f) {
-        u1 = 0.5f;
+    if (fabs(u1)> 0.6f) {  
+        u1 = 0.6f;
     } else {
         u1= u1;
     }
-    pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+    pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 void moter2_control(double u2)
@@ -224,20 +234,20 @@
     } else {
         u2= u2;
     }
-    pwmpin2= fabs(u2); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+    pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 void PwmMotor(void)
 {
+    float Polshoek1 = Kinematics1(Input1);
+    float Polshoek4 = Limits1(Polshoek1);
+    PolsReference = Polshoek4;
+    
     // Reference hoek berekenen, in graden
-    float Ellebooghoek1 = Kinematics2(pwm2);
+    float Ellebooghoek1 = Kinematics2(Input2);
     float Ellebooghoek4 = Limits2(Ellebooghoek1);
     ElbowReference = Ellebooghoek4;
     
-    float Polshoek1 = Kinematics1(pwm2);
-    float Polshoek4 = Limits1(Polshoek1);
-    PolsReference = Polshoek4;
-
     // Positie motor berekenen, in graden
     Pulses1 = encoder1.getPulses();
     motor_position1 = -(Pulses1/1200)*360;
@@ -341,18 +351,19 @@
         if (!But1)
         {
         led1 = 1;
-        float H1 = 0.98f;
+        float H1 = 0.7f;
         moter1_control(H1);
         wait(0.001f);
         }
         else if (!But2)
         {
         led1 = 1;
-        float H1 = -0.98f;
+        float H1 = -0.7f;
         moter1_control(H1);
         wait(0.001f);
         }
-        Pulses1 = 0;
+        encoder1.reset();
+        motor_position1 = 0;
         pwmpin1 = 0;
         pwmpin2 = 0;
         ;
@@ -360,7 +371,7 @@
         stateChanged = false;
       }
           
-      // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
+      // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF
 
         if (!button3)
         {        
@@ -386,11 +397,11 @@
     // Actions
       if (stateChanged)
       {
-        // state initialization: green
+        // state initialization: white
         t.start();
         led1 = 0;
-        led2 = 1;
-        led3 = 1;
+        led2 = 0;
+        led3 = 0;
         
         if (!But1)
         {
@@ -406,7 +417,8 @@
         moter2_control(H2);
         wait(0.001f);
         }
-        Pulses2 = 0;
+        encoder2.reset();
+        motor_position2 = 0;
         pwmpin1 = 0;
         pwmpin2 = 0;
         ;
@@ -467,8 +479,21 @@
       {
         // state initialization: purple
         t.start();
+
         pwmpin1 = 0;
         pwmpin2 = 0;
+        Input1 = pwm1;
+        Input2 = pwm2;
+                
+         // printen
+        if(count==500) 
+        {
+        float tmp = t2.read();
+        pc.printf(" Elleboog positie: %f reference: %f, rotatie positie: %f reference: \r\n", motor_position2, ElbowReference, motor_position1, PolsReference);
+        count = 0;
+        }
+        count++;
+        
         Pwm.attach (PwmMotor, Ts);
         led1 = 1;
         led2 = 0;
@@ -541,6 +566,7 @@
     led1 = 1;
     led2 =1;
     led3 =1;
+    /*
     if(counter==10) 
         {
         float tmp = t2.read();
@@ -548,7 +574,7 @@
         counter = 0;
         }
     counter++;
-    
+    */
     ProcessStateMachine();