Diff: main.cpp

Revision:

19:2d9421b0316a

Parent:

18:f36ac3ee081a

Child:

20:7f1997276ce2

--- a/main.cpp	Wed Oct 31 14:49:34 2018 +0000
+++ b/main.cpp	Wed Oct 31 15:33:40 2018 +0000
@@ -14,38 +14,55 @@
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
 DigitalOut led_blue(LED_BLUE);
+
 // Buttons
 DigitalIn button_clbrt_home(SW2);
 DigitalIn button_Demo(D5);
 DigitalIn button_Emg(D6);
 DigitalIn Fail_button(SW3);
+
 // Modserial
 MODSERIAL pc(USBTX, USBRX);
+
 // Encoders
 QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
 QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
+
 // Motors (direction and PWM)
 DigitalOut directionM1(D4);
 DigitalOut directionM2(D7);
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
+
 // Inverse Kinematics
-volatile double q1_diff;
-volatile double q2_diff;
+int track;
+volatile double  U1;
+volatile double  U2;
+DigitalIn directionx(PTC2); //x direction switch button
+DigitalIn directiony(PTA2); //y direction switch button
+const double r_big = 590.0; //maximum radius of the moving space
+const double r_small = 162.0; //minimum radius of the moving space
+const double r_top = 250.0; //radius of the top portion of the moving space
+double v=1.0; //moving speed of setpoint (dependant on the waiting time)
+volatile int sx;//value of the button and store as switch
+volatile int sy;//value of the button and store as switch
+int dirx = 1; //determine the direction of the setpoint placement
+int diry = 1; //determine the direction of the setpoint placement
+double q1_diff;
+double q2_diff;
 double sq = 2.0; //to square numbers
+const double x0 = 80.0; //zero x position after homing
+const double y0 = 141.0; //zero y position after homing
 const double L1 = 250.0; //length of the first link
 const double L3 = 350.0; //length of the second link
-int track;
-const double x0 = 80.0; //zero x position after homing
-const double y0 = 141.0; //zero y position after homing
-volatile double  setpointx = x0;
-volatile double  setpointy = y0;
-volatile double  U1;
-volatile double  U2;
+volatile double setpointx = x0; //sets the begin condition for x to x0
+volatile double setpointy = y0; //sets the begin condition for y to y0
+
 // Reference angles of the starting position
 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
 double q2_0_enc = q2_0 + q1_0;
+
 // EMG input en start value of filtered EMG
 AnalogIn    emg1_raw( A0 );
 AnalogIn    emg2_raw( A1 );
@@ -57,7 +74,7 @@
 Timer       timer;                        // Timer for counting time in this state
 Ticker      WriteValues;            // Ticker to show values of velocity to screen
 Ticker      StateMachine;
-//Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
+Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
 //HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope
 
 // Set up ProcessStateMachine
@@ -78,6 +95,8 @@
 float theta2_prev = 0.0;
 float tijd = 0.005;
 float time_in_state;
+volatile double  error1;
+volatile double  error2;
 
 int need_to_move_1;                     // Does the robot needs to move in the first direction?
 int need_to_move_2;                     // Does the robot needs to move in the second direction?
@@ -148,6 +167,50 @@
     return -q2_diff;
 }
 
+
+// ---------------------------------------------------
+// --------DETERMINE SETPOINT-------------------------
+// --------------------------------------------------- 
+//function that determines the setpoint of the x coordinate
+double EMG1On(int s){
+    if (setpointx < 80.0){ //minimum setpoint
+        setpointx = setpointx;}
+    if (setpointy > -66.0 && setpointy < 362.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) > r_big){ //defines the large circle endpoint
+        setpointx = setpointx;}
+    if (setpointy > 141.0 && setpointx < 540.0 && sqrt(pow(setpointy-119.2,sq)+pow(setpointx-329.0,sq)) > r_top){ //defines the top circle endpoint
+        setpointx = setpointx;}
+    if (setpointx > 80.0  && setpointy > -36.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) < r_small){ //defines the small circle endpoint
+        setpointx = setpointx;}
+    if (setpointy > -66.0 && setpointy < -36.0 && setpointx < 157.0){ //kleine stukje
+        setpointx =  setpointx+2;}
+    else setpointx = setpointx + dirx*s*v;
+    return setpointx;
+    }
+    
+//function that determines the setpoint of the y coordinate
+double EMG2On(int s){
+    if (setpointy < -66.0) //bottom line
+        setpointy = setpointy;
+    if (setpointy > -66.0 && setpointy < 362.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) > r_big){ //defines the large circle endpoint
+        setpointy = setpointy;}
+    if (setpointy >= 141.0 && setpointx < 540.0 && sqrt(pow(setpointy-119.2,sq)+pow(setpointx-329.0,sq)) > r_top){ //defines the top circle endpoint
+        setpointy = setpointy;}
+    if (setpointx > 80.0 && setpointy > -36.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) < r_small){ //defines the small circle endpoint
+        setpointy = setpointy;}
+    else setpointy = setpointy + diry*s*v;
+    return setpointy;
+    }
+
+//function to change the moving direction of the setpoint
+void ChangeDirectionX(){
+    dirx = -1*dirx;
+    }
+    
+void ChangeDirectionY(){
+    diry = -1*diry;
+    }
+
+
 // --------------------------------------------------------------------
 // ---------------READ-OUT ENCODERS------------------------------------
 // --------------------------------------------------------------------    
@@ -213,8 +276,8 @@
 // ------------ RUN MOTORS -----------------------
 // -----------------------------------------------
 void motoraansturing()
-{
-    determinedemoset();
+{   
+    
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
 
@@ -397,6 +460,8 @@
         // will the move to the corresponding state.
                 
         // BUILD DEMO MODE
+       // determinedemoset();
+        motoraansturing();
         
         if (button_clbrt_home == 0) 
         {
@@ -432,6 +497,9 @@
                 need_to_move_2 = 0;
                 }   
             
+            setpointx = EMG1On(need_to_move_1);         // Determine setpoints
+            setpointy = EMG2On(need_to_move_2);
+            motoraansturing();
             
             // Requirements to move to the next state:
             // When the home button or the failure button is pressed, we 
@@ -514,7 +582,12 @@
     sample_EMGtoHIDscope.attach(&sample, 0.02f);         // Display EMG values 50 times per second
     
     while (true) {
-        
+        if (currentState == MOVE_W_EMG){
+            InterruptIn directionx(PTC2);
+            directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
+            InterruptIn directiony(PTA2);
+            directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
+            }
     }
 }