Diff: main.cpp

Revision:

0:55f9447aa02b

Child:

1:a644028231b5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 26 10:33:14 2016 +0000
@@ -0,0 +1,177 @@
+#include "mbed.h"
+#include "PID.h"
+#include "encoder.h"
+#include "math.h"
+
+//D8 doet het niet
+
+DigitalIn    knop_1(D2); //Motor 1
+DigitalIn    knop_2(D3); //Motor 2
+
+DigitalOut motor_1(D7); //richting (1 of 0)
+PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
+DigitalOut motor_2(D4); //richting (1 of 0)
+PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
+InterruptIn stop(SW3); //stoppen
+
+Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
+
+Encoder encoder_1(D13,D12);
+Encoder encoder_2(D10,D9);
+
+Ticker PID_ticker;
+
+Timer tijd;
+
+//constante waardes
+const double x_start = -0.25;
+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 L2=0.35; //m
+const double L3=0.05; //m
+const double y_base = 0.3;
+//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
+volatile int mode;   //mode waarin de motors moeten bewegen
+//controller
+volatile double theta_1, theta_2, reference_1, plaats_1, error_1, pwm_1, reference_2, plaats_2, error_2, pwm_2;
+//beginpositie
+volatile double x = x_start; //m
+volatile double y = y_base; //m
+volatile double yc= y + L3 - L1; //m
+volatile bool opgepakt = false;
+volatile bool zakpunt = false;
+volatile bool uitdrukken = false;
+volatile double y_oppakken, y_stijgen, x_zakken;
+//PID
+const double m1_Kp = 2.0, m1_Ki = 0.0, m1_Kd = 0.0, 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;
+double m2_v1 = 0, m2_v2 = 0; // Memory variables
+const double m2_Ts = 0.01; // Controller sample time
+
+
+//Controller PID motors
+void Controller()
+{
+    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
+    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 );
+    if (pwm_1<0){motor_1 = 1;}
+    else {motor_1 = 0;}
+    pwm_motor_1 = fabs(pwm_1);
+
+    reference_2 = 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);
+}
+
+//Failsave
+void Stop() //Zet allebei de snelheden op nul
+{
+    pwm_motor_1 = 0;
+    pwm_motor_2 = 0;
+    while(true){}
+}
+
+int main()
+{
+    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
+    
+    while(true)
+    {
+    tijd_nieuw = tijd;
+    tijd_verschil = tijd_nieuw - tijd_oud;
+    tijd_oud = tijd_nieuw;
+    
+    if      (knop_1 == 0 && knop_2 == 1){mode = 1;} 
+    else if (knop_1 == 1 && knop_2 == 0){mode = 2;} 
+    else if (knop_1 == 0 && knop_2 == 0){mode = 3;}
+    else                              {mode = 4;} //default
+    
+    switch (mode)
+    {
+        case 1: x = x - tijd_verschil*Vx;
+                if (x<-0.5){x = -0.5;}
+                y = y_base;
+                yc= y + L3 - L1;
+                break;
+        case 2: x = x + tijd_verschil*Vx;
+                if (x>-0.25){x = -0.25;}
+                y = y_base;
+                yc= y + L3 - L1;
+                break;
+        case 3: while(y > (y_base-y_oppakken))    
+                {
+                    tijd_nieuw = tijd;
+                    tijd_verschil = tijd_nieuw - tijd_oud;        
+                    y = y - tijd_verschil*Vy;
+                    yc= y + L3 - L1;
+                    x = x;
+                    tijd_oud = tijd_nieuw;
+                }
+                while(y < (y_base+y_stijgen))
+                {
+                    tijd_nieuw = tijd;
+                    tijd_verschil = tijd_nieuw - tijd_oud;        
+                    y = y + tijd_verschil*Vy;
+                    yc= y + L3 - L1;
+                    x = x;
+                    tijd_oud = tijd_nieuw;
+                }
+                while(x > (x_start+x_zakken))
+                {
+                    tijd_nieuw = tijd;
+                    tijd_verschil = tijd_nieuw - tijd_oud;        
+                    x = x-tijd_verschil*Vx;
+                    yc = yc;
+                    tijd_oud = tijd_nieuw;
+                }
+                while(y>y_base)
+                {
+                    tijd_nieuw = tijd;
+                    tijd_verschil = tijd_nieuw - tijd_oud;    
+                    y = y - tijd_verschil*Vy;
+                    yc= y + L3 - L1;
+                    x = x;
+                    tijd_oud = tijd_nieuw;
+                }
+                while((x>x_start) && uitdrukken)
+                {
+                    tijd_nieuw = tijd;
+                    tijd_verschil = tijd_nieuw - tijd_oud;   
+                    y = y_base;
+                    yc= y + L3 - L1;
+                    x = x - tijd_verschil*Vx;
+                    tijd_oud = tijd_nieuw;
+                }
+                    x = x_start;
+                    y = y_base;
+                    yc= y + L3 - L1;
+                break;
+        default: x = x;
+                 yc = yc;
+                 break;
+    }    
+    }
+}
\ No newline at end of file