Diff: main.cpp

Revision:

6:23880486c8fc

Parent:

5:0bbb52e29790

Child:

7:29c93152d96a

--- a/main.cpp	Wed Oct 31 18:34:20 2018 +0000
+++ b/main.cpp	Thu Nov 01 09:31:35 2018 +0000
@@ -26,6 +26,12 @@
 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);
+double rotation_end_position=1;
+double tower_1_position=1;
+double tower_end_position=1;
+const double q1start = rotation_end_position * alpha;
+const double q2start = tower_1_position * beta; //!!!!afhankelijk van een variabelle die in anderscript wordt bepaald!!!!
+const double q2end = tower_end_position * beta; //!!!!afhankelijk van een variabelle die in anderscript wordt bepaald!!!!
 
 //variables motors
 double out1;
@@ -79,6 +85,15 @@
 return u_k + u_i + u_d;
 }
 
+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()
 {
     out1 = (pot1*2.0f)-1.0f; //control x-direction
@@ -87,14 +102,16 @@
     counts2 = encoder2.getPulses(); //counts encoder 2
     vdesx = out1 * 20.0; //speed x-direction
     vdesy = out2 * 20.0; //speed y-direction
-    q1 = counts1 * alpha; //counts to rotation (rad) 
-    q2 = counts2 * beta; //counts to translation (mm)
+
+    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; //
@@ -104,7 +121,8 @@
     dirpin2.write(pwm2 < 0);
     pwmpin2 = fabs (pwm2);
 }
-    
+
+
 //main 
 int main(){
     pc.baud(115200);