Diff: main.cpp

Revision:

17:e5d9a543157b

Parent:

9:8b2d6ec577e3

Child:

18:f36ac3ee081a

--- a/main.cpp	Wed Oct 31 10:30:34 2018 +0000
+++ b/main.cpp	Wed Oct 31 14:44:13 2018 +0000
@@ -3,14 +3,13 @@
 #include "QEI.h"
 #include "FastPWM.h"
 #include "math.h"
-
-#include "mbed.h"
 //#include "HIDScope.h"
 #include "BiQuad.h"
 #include "BiQuad4.h"
 #include "FilterDesign.h"
 #include "FilterDesign2.h"
 
+const double  pi = 3.14159265359;
 // LED's
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
@@ -30,6 +29,23 @@
 DigitalOut directionM2(D7);
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
+// Inverse Kinematics
+volatile double q1_diff;
+volatile double q2_diff;
+double sq = 2.0; //to square numbers
+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;
+// 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 );
@@ -60,7 +76,6 @@
 float vel_2;
 float theta1_prev = 0.0;
 float theta2_prev = 0.0;
-const float pi = 3.14159265359;
 float tijd = 0.005;
 float time_in_state;
 
@@ -113,6 +128,108 @@
     scope.send();                       // Send the data to the computer
     */
 }
+
+// ---------------------------------------------------
+// --------INVERSE-KINEMATICS-------------------------
+// --------------------------------------------------- 
+double makeAngleq1(double x, double y){
+    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+    q1_diff = -2.0*(q1-q1_0);                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return q1_diff;
+}
+
+double makeAngleq2(double x, double y){
+    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));                                  //angle of the second joint in setpoint configuration
+    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+    double q2_motor = (pi - q2)+q1;                                                                              //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+    q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi);                                                              //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return -q2_diff;
+}
+
+// --------------------------------------------------------------------
+// ---------------READ-OUT ENCODERS------------------------------------
+// --------------------------------------------------------------------    
+double  counts2angle1()
+{
+    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
+    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    return theta1;
+}
+
+double counts2angle2()
+{
+    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
+    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    return theta2;
+}
+
+// -----------------------------------------------------------------    
+// --------------------------- PI controllers ----------------------
+// -----------------------------------------------------------------
+double PI_controller1(double error1)
+{
+    static double error_integral1 = 0;
+    
+    // Proportional part
+    double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral1 = error_integral1 + error1 * Ts1;  
+    double u_i1 = Ki1 * error_integral1;
+    
+    // Sum 
+    U1 = u_p1 + u_i1;
+    
+    // Return
+    return U1;      
+}
+double PI_controller2(double error2)
+{
+    static double error_integral2 = 0;
+    
+    // Proportional part
+    double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral2 = error_integral2 + error2 * Ts2;  
+    double u_i2 = Ki2 * error_integral2;
+    
+    // Sum +
+    U2 = u_p2 + u_i2;
+    
+    // Return
+    return U2;      
+} 
+
+// -----------------------------------------------
+// ------------ RUN MOTORS -----------------------
+// -----------------------------------------------
+void motoraansturing()
+{
+    determinedemoset();
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+
+    theta2 = counts2angle2();           
+    error2 = q2_diff - theta2;
+    theta1 = counts2angle1();  
+    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+
+    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PI_controller2(error2);   
+    
+    motor1_pwm.write(fabs(U1));         // Motor aansturen
+    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
+    motor2_pwm.write(fabs(U2));
+    directionM2 = U2 > 0.0f; 
+}
+
 // ---------------------------------------------------
 // --------STATEMACHINE------------------------------- 
 // --------------------------------------------------- 
@@ -121,146 +238,146 @@
     switch (currentState)
     {
         case WAITING:
-        // Description:
-        // In this state we do nothing, and wait for a command
-        
-        // Actions        
-        led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
-        
-        // State transition logic
-        if (button_clbrt_home == 0) 
-        {
-            currentState = MOTOR_ANGLE_CLBRT;
-            stateChanged = true;
-            pc.printf("Starting Calibration\n\r");
-        }
-        else if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
-        break;
+            // Description:
+            // In this state we do nothing, and wait for a command
+            
+            // Actions        
+            led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
+            
+            // State transition logic
+            if (button_clbrt_home == 0) 
+            {
+                currentState = MOTOR_ANGLE_CLBRT;
+                stateChanged = true;
+                pc.printf("Starting Calibration\n\r");
+            }
+            else if (Fail_button == 0)
+            {
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
+            break;
                 
         case MOTOR_ANGLE_CLBRT:
-        // Description:
-        // In this state the robot moves with low motor PWM to some 
-        // mechanical limit of motion, in order to calibrate the motors.
-         
-        // Actions       
-        led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
-        timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
-        if (stateChanged)
-            {
-                MotorAngleCalibrate();                      // Actuate motor 1 and 2.
-                vel_1 = ReadEncoder1();                     // Get velocity of motor 1    
-                vel_2 = ReadEncoder2();                     // Get velocity of motor 2
-                stateChanged = true;                        // Keep this loop going, until the transition conditions are satisfied.    
-            }
-    
-        // State transition logic
-        time_in_state = timer.read();                       // Determine if this state has run for long enough.
-    
-        if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
+            // Description:
+            // In this state the robot moves with low motor PWM to some 
+            // mechanical limit of motion, in order to calibrate the motors.
+             
+            // Actions       
+            led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
+            timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
+            if (stateChanged)
+                {
+                    MotorAngleCalibrate();                      // Actuate motor 1 and 2.
+                    vel_1 = ReadEncoder1();                     // Get velocity of motor 1    
+                    vel_2 = ReadEncoder2();                     // Get velocity of motor 2
+                    stateChanged = true;                        // Keep this loop going, until the transition conditions are satisfied.    
+                }
+        
+            // State transition logic
+            time_in_state = timer.read();                       // Determine if this state has run for long enough.
+        
+            if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
+                {
+                    //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);           
+                    //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
+                    pc.printf("Motor calibration has ended. \n\r");
+                    timer.stop();                              // Stop timer for this state
+                    timer.reset();                             // Reset timer for this state
+                    motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
+                    motor2_pwm.write(fabs(0.0));
+                    Encoder1.reset();                          // Reset Encoders when arrived at zero-position
+                    Encoder2.reset();
+            
+                    currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
+                    pc.printf("EMG calibration \r\n");
+                    stateChanged = true;                       
+               }
+            if (Fail_button == 0)
             {
-                //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);           
-                //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
-                pc.printf("Motor calibration has ended. \n\r");
-                timer.stop();                              // Stop timer for this state
-                timer.reset();                             // Reset timer for this state
-                motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
-                motor2_pwm.write(fabs(0.0));
-                Encoder1.reset();                          // Reset Encoders when arrived at zero-position
-                Encoder2.reset();
-        
-                currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
-                pc.printf("EMG calibration \r\n");
-                stateChanged = true;                       
-           }
-        if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
-        break; 
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
+            break; 
                         
         case EMG_CLBRT:
-        // In this state the person whom is connected to the robot needs 
-        // to flex his/her muscles as hard as possible, in order to 
-        // measure the maximum EMG-signal, which can be used to scale 
-        // the EMG-filter.
-                
-         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
-                
-         // Requirements to move to the next state:
-         // If enough time has passed (5 sec), and the EMG-signal drops below 10%
-         // of the maximum measured value, we move to the Homing state.
+            // In this state the person whom is connected to the robot needs 
+            // to flex his/her muscles as hard as possible, in order to 
+            // measure the maximum EMG-signal, which can be used to scale 
+            // the EMG-filter.
+                    
+             led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
                     
-         wait(5.0f); // time_in_this_state > 5.0f
-         // INSERT CALIBRATING
-         currentState = HOMING;
-        if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
-         break;       
+             // Requirements to move to the next state:
+             // If enough time has passed (5 sec), and the EMG-signal drops below 10%
+             // of the maximum measured value, we move to the Homing state.
+                        
+             wait(5.0f); // time_in_this_state > 5.0f
+             // INSERT CALIBRATING
+             currentState = HOMING;
+            if (Fail_button == 0)
+            {
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
+             break;       
                             
         case HOMING:
-        // Description:
-        // Robot moves to the home starting configuration
-        pc.printf("HOMING \r\n");
+            // Description:
+            // Robot moves to the home starting configuration
+            pc.printf("HOMING \r\n");
+                        
+            led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
+                    
+            // Requirements to move to the next state:
+            // If we are in the right location, within some margin, we move to the Waiting for
+            // signal state.
                     
-        led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
-                
-        // Requirements to move to the next state:
-        // If we are in the right location, within some margin, we move to the Waiting for
-        // signal state.
-                
-        wait(5.0f); // time_in_this_state > 5.0f
-        // INSERT MOVEMENT
-        currentState = WAITING4SIGNAL;
-        if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
-        break;
+            wait(5.0f); // time_in_this_state > 5.0f
+            // INSERT MOVEMENT
+            currentState = WAITING4SIGNAL;
+            if (Fail_button == 0)
+            {
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
+            break;
                 
         case WAITING4SIGNAL:
-        // Description:
-        // In this state the robot waits for an action to occur.
-        
-        led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
-                
-        // Requirements to move to the next state:
-        // If a certain button is pressed we move to the corresponding 
-        // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
-                        
-        if (button_clbrt_home == 0) 
-        {
-            currentState = MOTOR_ANGLE_CLBRT;
-            stateChanged = true;
-            pc.printf("Starting Calibration \n\r");
-        }
-        else if (button_Demo == 1) 
-        {
-            currentState = MOVE_W_DEMO;
-            pc.printf("DEMO \r\n");
-            wait(1.0f);
-        }
-        else if (button_Emg == 1) 
-        {
-            currentState = MOVE_W_EMG;
-            pc.printf("EMG \r\n");
-            wait(1.0f);
-        }
-        else if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
-        
-        break;
+            // Description:
+            // In this state the robot waits for an action to occur.
+            
+            led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
+                    
+            // Requirements to move to the next state:
+            // If a certain button is pressed we move to the corresponding 
+            // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
+                            
+            if (button_clbrt_home == 0) 
+            {
+                currentState = MOTOR_ANGLE_CLBRT;
+                stateChanged = true;
+                pc.printf("Starting Calibration \n\r");
+            }
+            else if (button_Demo == 1) 
+            {
+                currentState = MOVE_W_DEMO;
+                pc.printf("DEMO \r\n");
+                wait(1.0f);
+            }
+            else if (button_Emg == 1) 
+            {
+                currentState = MOVE_W_EMG;
+                pc.printf("EMG \r\n");
+                wait(1.0f);
+            }
+            else if (Fail_button == 0)
+            {
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
+            
+            break;
                 
         case MOVE_W_DEMO:
         // Description:
@@ -289,57 +406,59 @@
         break;
                 
         case MOVE_W_EMG:
-        // Description:
-        // In this state the robot will be controlled by use of 
-        // EMG-signals. 
-                    
-        led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
-             
-        if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
-            need_to_move_1 = 1; // The robot does have to move
-            }
-        else {
-            need_to_move_1 = 0; // If the robot does not have to move
-            }
-
-        if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
-            need_to_move_2 = 1;
+            // Description:
+            // In this state the robot will be controlled by use of 
+            // EMG-signals. 
+                        
+            led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
+                 
+            if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
+                need_to_move_1 = 1; // The robot does have to move
+                }
+            else {
+                need_to_move_1 = 0; // If the robot does not have to move
+                }
+    
+            if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
+                need_to_move_2 = 1;
+                }
+            else {
+                need_to_move_2 = 0;
+                }   
+            
+            
+            // Requirements to move to the next state:
+            // When the home button or the failure button is pressed, we 
+            // will the move to the corresponding state.
+            
+            if (button_clbrt_home == 0) 
+            {
+                currentState = MOTOR_ANGLE_CLBRT;
+                stateChanged = true;
+                pc.printf("Starting Calibration \n\r");
+            }        
+            else if (Fail_button == 0)
+            {
+                currentState = FAILURE_MODE;
+                stateChanged = true;
             }
-        else {
-            need_to_move_2 = 0;
-            }   
-        // Requirements to move to the next state:
-        // When the home button or the failure button is pressed, we 
-        // will the move to the corresponding state.
-        
-        if (button_clbrt_home == 0) 
-        {
-            currentState = MOTOR_ANGLE_CLBRT;
-            stateChanged = true;
-            pc.printf("Starting Calibration \n\r");
-        }        
-        else if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
-        break;      
+            break;      
                  
         case FAILURE_MODE:
-        // Description:
-        // This state is reached when the failure button is reached. 
-        // In this state everything is turned off.
-            
-        led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
-        // Actions
-        if (stateChanged)
-        {
-            motor1_pwm.write(fabs(0.0));               // Stop all motors!
-            motor2_pwm.write(fabs(0.0));
-            pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
-            stateChanged = false;
-        }    
-        break;
+            // Description:
+            // This state is reached when the failure button is reached. 
+            // In this state everything is turned off.
+                
+            led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
+            // Actions
+            if (stateChanged)
+            {
+                motor1_pwm.write(fabs(0.0));               // Stop all motors!
+                motor2_pwm.write(fabs(0.0));
+                pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
+                stateChanged = false;
+            }    
+            break;
         
         // State transition logic
         // No state transition, you need to restart the robot.
@@ -386,9 +505,8 @@
     pc.printf("WAITING... \r\n");
     
     StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
-    /**
     sample_EMGtoHIDscope.attach(&sample, 0.02f);         // Display EMG values 50 times per second
-    */
+    
     while (true) {
         
     }