Diff: THE.cpp

Revision:

16:37b491eac34b

Parent:

15:2772f8cbf382

Child:

17:65943f6e11dc

--- a/THE.cpp	Thu Nov 01 11:00:14 2018 +0000
+++ b/THE.cpp	Thu Nov 01 11:14:44 2018 +0000
@@ -59,7 +59,7 @@
 BiQuad L2( c1, c2, d0, d1, d2);
 BiQuad L3( c1, c2, d0, d1, d2);
 
-const float T=0.001f;
+const float T = 0.001f;
 
 // EMG
 const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
@@ -90,6 +90,54 @@
 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
 PwmOut pwmpin_2(D6); // PWM pin of motor 2
 
+// RKI related
+const double Ts = 0.001;// sample frequency 
+
+// Constants motor
+const double delta_t = 0.01;
+const double L1 = 370.0 / 2.0;
+const double L2 = 65.0 / 2.0;
+const double pi = 3.14159265359;
+const double alpha = (2.0 * pi) /(25.0*8400.0);
+const double beta = (((2.0 * L1) - (2.0 * L2)) * 20.0 * pi) / (305.0 * 8400.0);
+const double q1start = rotation_end_position * alpha;
+const double q2start = tower_1_position * beta; 
+const double q2end = tower_end_position * beta; 
+
+// Variables motors
+double out1;
+double out2;
+int counts1;
+int counts2;
+double vdesx;
+double vdesy;
+double q1;
+double q2;
+double MPe;
+double xe;
+double ye;
+double gamma;
+double dq1;
+double dq2;
+double dC1;
+double dC2;
+double pwm1;
+double pwm2;
+
+// PID rotation constants 
+double Rot_Kp = 1.5;
+double Rot_Ki = 0.1;
+double Rot_Kd = 0.48;
+double Rot_error = 0;
+double Rot_prev_error = 0;
+
+// PID translation constants
+const double Trans_Kp = 0.5;
+const double Trans_Ki = 0.5;
+const double Trans_Kd = 0.1;
+double Trans_error = 0;
+double Trans_prev_error = 0;
+
 // Extra stuff
 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
@@ -529,9 +577,22 @@
 // PID controller
 // To control the input signal before it goes into the motor control
 // Simon mee bezig
-void pid_controller()
-{
-    // Simons code here
+// PID execution
+double PID_control(double error, const double kp, const double ki, const double kd, double &error_int, double &error_prev)
+{ 
+    // P control
+    double u_k = kp * error;
+
+    // I control
+    error_int = error_int + (Ts * error);
+    double u_i = ki * error_int;
+
+    // D control
+    double error_deriv = (error - error_prev);
+    double u_d = kd * error_deriv;
+    error_prev = error;
+
+    return u_k + u_i + u_d;
     }
 
 // START
@@ -560,11 +621,48 @@
 // OPERATING
 // To control the robot with EMG signals
 // Kenneth bezig met motor aansturen
-void operating()
+void boundaries()
+{
+    double q2tot = q2 + dq2;
+    if (q2tot > q2end)
+    {
+        dq2 = 0;
+        } //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end?
+    else if (q2tot < q2start)
+    {
+        dq2 = 0;
+        }
+    else{}
+    }
+
+void motor_control()
 {
-    servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker? 
-    // RKI fuction
-    }    
+    // servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker?
+    out1 = (pot1*2.0f)-1.0f; //control x-direction
+    out2 = (pot2*2.0f)-1.0f; //control y-direction
+    counts1 = encoder1.getPulses(); //counts encoder 1
+    counts2 = encoder2.getPulses(); //counts encoder 2
+    vdesx = out1 * 20.0; //speed x-direction
+    vdesy = out2 * 20.0; //speed y-direction
+
+    q1 = counts1 * alpha + q1start; //counts to rotation (rad) 
+    q2 = counts2 * beta + q2start; //counts to translation (mm)
+    MPe = L1 - L2 + q2; //x location end effector, x-axis along the translation
+    xe = cos(q1) * MPe; //x location in frame 0
+    ye = sin(q1) * MPe; //y location in frame 0
+    gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
+    dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
+    dq2 = gamma * delta_t * (-1.0 * xe * vdesx - ye * vdesy); //target translation
+    boundaries();
+    dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts
+    dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
+    pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
+    pwm2 = 3.0 * (dC2 / delta_t) / 8400.0; //
+    dirpin.write(pwm1 < 0);                    
+    pwmpin = fabs (pwm1);                    
+    dirpin2.write(pwm2 < 0);
+    pwmpin2 = fabs (pwm2);
+    }   
 
 // DEMO
 // To control the robot with a written code and write 'HELLO'
@@ -744,6 +842,12 @@
             if(StateChanged) // so if StateChanged is true
             {
                 // Execute OPERATING mode
+                while(1)
+                {
+                    motor_control();
+                    pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy);
+                    wait(delta_t);
+                    }
                 
                 StateChanged = false; // state is still OPERATING
                 }