Diff: THE.cpp

Revision:

32:fad4b119dce0

Parent:

31:969635fd7f57

Child:

33:a03bb006dff4

--- a/THE.cpp	Fri Nov 02 10:43:27 2018 +0000
+++ b/THE.cpp	Fri Nov 02 11:41:41 2018 +0000
@@ -760,80 +760,16 @@
 // 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
-    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);
-    } 
 
 // DEMO
 // Executing demo mode, drawing a square
 void demo_mode()
 {
-    while(true)
-    {
-        demo_control(-pi/4.0,-pi/4.0);
-        if ((Counts2(counts2) > -26267-1000 )&& (Counts2(counts2) < -26267+1000))
-        {
-            
-            rotation_stop();
-            
-        }
-        if ((Counts1(counts1) > 13369-1000)&& (Counts1(counts1) < 13369+1000))
-        {translation_stop();}
-        if ((Counts2(counts2) > -26267-1000 )&& (Counts2(counts2) < -26267+1000)&& (Counts1(counts1) > 13369-1000)&& (Counts1(counts1) < 13369+1000))
-        {break;}
-        }
-        
-    while(true)
-    {
-        demo_control(pi,pi);
-        if ((Counts2(counts2) > -78802-1000 )&& (Counts2(counts2) < -78802+1000))
-        {
-            
-            rotation_stop();
-        }
-        if ((Counts1(counts1) > 13369-1000)&& (Counts1(counts1) <  13369+1000))
-            {translation_stop();
-        }
-        if ((Counts2(counts2) > -78802-1000 )&& (Counts2(counts2) < -78802+1000) && (Counts1(counts1) > 13369-1000)&& (Counts1(counts1) <  13369+1000))
-        { break;
-        }
-    }
-        
-    while(true)
-    {
-        demo_control(pi/4.0,pi/4.0);
-        if ((Counts2(counts2) > -105069-1000 )&& (Counts2(counts2) < -105069+1000))
-        {
-            
-            rotation_stop();
-            
-            }
-        if ((Counts1(counts1) > 24732-1000)&& (Counts1(counts1) < 24732+1000))
-        {translation_stop();}
-        if ((Counts2(counts2) > -105069-1000 )&& (Counts2(counts2) < -105069+1000)&& (Counts1(counts1) > 24732-1000)&& (Counts1(counts1) < 24732+1000))
-        {break;}
-    }
+    translation_start(0,0.9);
+    rotation_start(0,0.7); 
+    wait(1);
+    translation_stop();
+    rotation_stop();
 }
 
 
@@ -884,10 +820,10 @@
             case WAIT:
             
             StateChanged = true;
-            pc.printf("\r\nState is WAIT\r\n");
             
             if(StateChanged) // so if StateChanged is true
             {
+                pc.printf("\r\nState is WAIT\r\n");
                 // Execute WAIT mode
                 wait_mode();
                 
@@ -978,7 +914,6 @@
                     if(button_wait == false) // condition OPERATING --> WAIT; button_wait press  
                     {
                         CurrentState = WAIT;
-                        pc.printf("\r\nState is WAIT\r\n");
                         g = false;
                         break;
                         }
@@ -1014,7 +949,6 @@
             if(button_wait == false) // condition OPERATING --> WAIT; button_wait press  
             {
                 CurrentState = WAIT;
-                pc.printf("\r\nState is WAIT\r\n");
                 StateChanged = false;
                 }