Diff: THE.cpp

Revision:

28:1481896462ee

Parent:

27:399ca1f28bd8

Child:

29:344d11928bf5

diff -r 399ca1f28bd8 -r 1481896462ee THE.cpp
--- a/THE.cpp	Fri Nov 02 10:00:44 2018 +0000
+++ b/THE.cpp	Fri Nov 02 10:03:01 2018 +0000
@@ -760,8 +760,63 @@
 // DEMO
 // To control the robot with a written code and write 'HELLO'
 // Voor het laatst bewaren
+void demo_control(double a,double  b){
+    out1 = cos(a); //control x-direction
+    out2 = sin(b); //control y-direction
+    Directioncontrol();
+    vdesx = out1 * 20.0; //speed x-direction
+    vdesy = out2 * 20.0; //speed y-direction
+
+    q1 = Counts2(counts2) * alpha + q1start; //counts to rotation (rad) 
+    q2 = Counts1(counts1)* beta + q2start; //counts to translation (mm)
+    MPe = el_1 - el_2 + 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 * (xe * vdesx + ye * vdesy) * -1.0; //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 = 7.0 * (dC2 / delta_t) / 8400.0; //
+    dirpin_1.write(pwm2 < 0); // translatie                 
+    pwmpin_1 = fabs (pwm2);                    
+    dirpin_2.write(pwm1 < 0); // rotatie
+    pwmpin_2 = fabs (pwm1);
+    } 
+
 void demo_mode()
 {
+    
+    
+    while(true){
+    demo_control(-pi/4.0,-pi/4.0);
+    if ((Counts1(counts1) > -26267-1000 )&& (Counts1(counts1) < -26267+1000)&& (Counts2(counts2) > 13369-1000)&& (Counts2(counts2) < 13369+1000)){
+        translation_stop()
+        rotation_stop()
+        break;
+        }
+    }
+        
+    while(true){
+    demo_control(pi,pi);
+    if ((Counts1(counts1) > -78802-1000 )&& (Counts1(counts1) < -78802+1000)&& (Counts2(counts2) > 13369-1000)&& (Counts2(counts2) <  13369+1000)){
+        translation_stop()
+        rotation_stop()
+        break;
+        }
+    }
+        
+    while(true){
+    demo_control(pi/4.0,pi/4.0);
+    if ((Counts1(counts1) > -105069-1000 )&& (Counts1(counts1) < -105069+1000)&& (Counts2(counts2) > 24732-1000)&& (Counts2(counts2) < 24732+1000)){
+        translation_stop()
+        rotation_stop()
+        break;
+        }
+    }
+}
     // code here
     }