Diff: main.cpp

Revision:

1:a644028231b5

Parent:

0:55f9447aa02b

Child:

2:a42b34f9d6ab

--- a/main.cpp	Wed Oct 26 10:33:14 2016 +0000
+++ b/main.cpp	Thu Oct 27 13:51:14 2016 +0000
@@ -2,6 +2,7 @@
 #include "PID.h"
 #include "encoder.h"
 #include "math.h"
+#include "HIDScope.h"
 
 //D8 doet het niet
 
@@ -19,19 +20,21 @@
 Encoder encoder_1(D13,D12);
 Encoder encoder_2(D10,D9);
 
+HIDScope scope(4);
+
 Ticker PID_ticker;
 
 Timer tijd;
 
 //constante waardes
-const double x_start = -0.25;
+const double x_start = 0.27;
 const double dt = 0.001;
 const double Vx = 0.05; //m/s
-const double Vy = 0.01; //m/s
-const double L1=0.25; //m
+const double Vy = 0.03; //m/s
+const double L1=0.30; //m
 const double L2=0.35; //m
-const double L3=0.05; //m
-const double y_base = 0.3;
+const double L3=0.13; //m
+const double y_base = 0.22;
 //filter
 volatile double tijd_oud, tijd_nieuw, tijd_verschil;
 volatile double u_1, y_1, u_2, y_2; //ongefilerd signaal emg 1, gefilterd signaal emg 1, ongefilterd signaal emg 2, gefilterd signaal emg 2
@@ -45,12 +48,12 @@
 volatile bool opgepakt = false;
 volatile bool zakpunt = false;
 volatile bool uitdrukken = false;
-volatile double y_oppakken, y_stijgen, x_zakken;
+volatile double y_oppakken = 0.125, y_stijgen = 0.125, x_zakken = 0.1;
 //PID
-const double m1_Kp = 2.0, m1_Ki = 0.0, m1_Kd = 0.0, m1_N = 50;
+const double m1_Kp = 20.0, m1_Ki = 0.5, m1_Kd = 0.6, m1_N = 50;
 double m1_v1 = 0, m1_v2 = 0; // Memory variables
 const double m1_Ts = 0.01; // Controller sample time
-const double m2_Kp = 2.0, m2_Ki = 0.0, m2_Kd = 0.0, m2_N = 50;
+const double m2_Kp = 20.0, m2_Ki = 0.5, m2_Kd = 0.6, m2_N = 50;
 double m2_v1 = 0, m2_v2 = 0; // Memory variables
 const double m2_Ts = 0.01; // Controller sample time
 
@@ -61,7 +64,7 @@
     theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
     theta_2 = acos((x/L2)-cos(theta_1));
 
-    reference_1 = theta_1;     //reference
+    reference_1 = -1.5*theta_1;     //reference
     plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
     error_1 = reference_1 - plaats_1 ;
     pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
@@ -69,13 +72,18 @@
     else {motor_1 = 0;}
     pwm_motor_1 = fabs(pwm_1);
 
-    reference_2 = theta_2;     //reference
+    reference_2 = -1.5*theta_2;     //reference
     plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
     error_2 = reference_2 - plaats_2 ;
     pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 );
     if (pwm_2<0){motor_2 = 0;}
     else {motor_2 = 1;}
     pwm_motor_2 = fabs(pwm_2);
+    scope.set(0,pwm_1);
+    scope.set(1,error_1);
+    scope.set(2,pwm_2);
+    scope.set(3,error_2);
+    scope.send();
 }
 
 //Failsave
@@ -90,13 +98,15 @@
 {
     pc.baud(115200);
     stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
-    PID_ticker.attach(&Controller,dt);
     tijd.reset();
     tijd.start();
     theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
     theta_2 = acos((x/L2)-cos(theta_1));
-    encoder_1.setPosition(theta_1/0.0014959); //positie encodercounts naar hoek
-    encoder_2.setPosition(theta_2/0.0014959); //positie encodercounts naar hoek
+    double positie_motor_1 = -1.5*theta_1;
+    double positie_motor_2 = -1.5*theta_2;
+    encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
+    encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
+    PID_ticker.attach(&Controller,dt);
     
     while(true)
     {
@@ -111,13 +121,13 @@
     
     switch (mode)
     {
-        case 1: x = x - tijd_verschil*Vx;
-                if (x<-0.5){x = -0.5;}
+        case 1: x = x + tijd_verschil*Vx;
+                if (x>0.6){x = 0.6;}
                 y = y_base;
                 yc= y + L3 - L1;
                 break;
-        case 2: x = x + tijd_verschil*Vx;
-                if (x>-0.25){x = -0.25;}
+        case 2: x = x - tijd_verschil*Vx;
+                if (x<x_start){x = x_start;}
                 y = y_base;
                 yc= y + L3 - L1;
                 break;
@@ -125,7 +135,7 @@
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;        
-                    y = y - tijd_verschil*Vy;
+                    y = y - tijd_verschil*Vy*4;
                     yc= y + L3 - L1;
                     x = x;
                     tijd_oud = tijd_nieuw;
@@ -134,7 +144,7 @@
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;        
-                    y = y + tijd_verschil*Vy;
+                    y = y + tijd_verschil*Vy*2;
                     yc= y + L3 - L1;
                     x = x;
                     tijd_oud = tijd_nieuw;
@@ -156,7 +166,7 @@
                     x = x;
                     tijd_oud = tijd_nieuw;
                 }
-                while((x>x_start) && uitdrukken)
+                while(x>x_start-0.04)
                 {
                     tijd_nieuw = tijd;
                     tijd_verschil = tijd_nieuw - tijd_oud;   
@@ -165,6 +175,7 @@
                     x = x - tijd_verschil*Vx;
                     tijd_oud = tijd_nieuw;
                 }
+                wait(0.5);
                     x = x_start;
                     y = y_base;
                     yc= y + L3 - L1;