Diff: main.cpp

Revision:

22:239075a92d33

Parent:

21:0d86a17f9974

Child:

23:d1a3d537460f

--- a/main.cpp	Wed Oct 30 22:43:24 2019 +0000
+++ b/main.cpp	Wed Oct 30 23:02:29 2019 +0000
@@ -44,6 +44,29 @@
 HIDScope scope(2);                  // Amount of HIDScope servers
 
 
+   double theta_motor1;
+    double theta_motor2;
+    double pi=3.14159265358979323846;
+    double length_link1=18;
+    double length_link2=15;
+    double x = 8.5;   // (of -2?) 8.5 < x < 30.5
+    double x_max= 30.5;
+    double x_min= 8.5;
+    double x_vergroting=0.01;
+    double y = 7.5; // 7.5 < y < 27
+    double y_max=27;
+    double y_min=7.5;
+    double y_vergroting=0.01;
+    double a;
+    double b;
+    double c;
+    double d;
+    
+    bool emg0_active;
+    bool emg1_active;
+    bool emg2_active;
+    bool emg3_active;
+
 // 3.3 EMG Variables **********************************************************
 static BiQuad LowPassFilter_motor1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
 static BiQuad LowPassFilter_motor2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
@@ -152,8 +175,8 @@
     // 4.1 Hidscope ****************************************************************
     void HIDScope() //voor HIDscope
     {
-        scope.set(0, 1);
-   //     scope.set(1, error1_motor1);
+        scope.set(0, x);
+        scope.set(1, y);
    //     scope.set(2, emg_tijd); 
    //     scope.set(3, emg3_signal);
    //    scope.set(4, Ui_motor1);
@@ -165,7 +188,7 @@
     // 4.x Encoder motor1 ****************************************************************
         double fencoder_motor1()                                    // bepaalt de positie van de motor
         {
-            positie_motor1 = encoder_motor1.getPulses();                // haalt encoder waardes op
+            positie_motor1 = encoder_motor1.getPulses()/counts_per_rad_motor1;                // haalt encoder waardes op
             positie_verschil_motor1 =  (positie_motor1-positie_prev_motor1)/Ts;
             positie_prev_motor1 = positie_motor1;
             
@@ -174,7 +197,7 @@
     // 4.x Encoder motor2 ****************************************************************        
         double fencoder_motor2()                                    // bepaalt de positie van de motor
         {
-            positie_motor2 = encoder_motor2.getPulses();                // haalt encoder waardes op
+            positie_motor2 = encoder_motor2.getPulses()/counts_per_rad_motor2;                // haalt encoder waardes op
             positie_verschil_motor2 =  (positie_motor2-positie_prev_motor2)/Ts;
             positie_prev_motor2 = positie_motor2;
             
@@ -222,16 +245,16 @@
     double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
     {
         //Proportional part
-        kp_motor1 = 0.01 ;      // moet nog getweaked worden
+        kp_motor1 = 1 ;      // moet nog getweaked worden
         Up_motor1 = kp_motor1 * error1_motor1;
     
         //Integral part
-        Ki_motor1 = 0.001;     // moet nog getweaked worden
+        Ki_motor1 = 0.1;     // moet nog getweaked worden
         error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
         Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
     
         //Derivative part 
-        Kd_motor1 = 0.001 ;// moet nog getweaked worden  
+        Kd_motor1 = 0.1 ;// moet nog getweaked worden  
         error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
         error1_derivative_filtered_motor1 = LowPassFilter_motor1.step(error1_derivative_motor1); //derivative wordt gefiltered
         Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
@@ -248,16 +271,16 @@
     double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
     {
         //Proportional part
-        kp_motor2 = 0.01 ;      // moet nog getweaked worden
+        kp_motor2 = 1 ;      // moet nog getweaked worden
         Up_motor2 = kp_motor2 * error1_motor2;
     
         //Integral part
-        Ki_motor2 = 0.001;     // moet nog getweaked worden
+        Ki_motor2 = 0.1;     // moet nog getweaked worden
         error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2);       // integrale fout + (de sample tijd * fout)
         Ui_motor2 = Ki_motor2 * error_integral_motor2;                     //de fout keer de integrale fout
     
         //Derivative part 
-        Kd_motor2 = 0.001 ;// moet nog getweaked worden  
+        Kd_motor2 = 0.1 ;// moet nog getweaked worden  
         error1_derivative_motor2 = (error1_motor2  -  error1_prev_motor2)/Ts;
         error1_derivative_filtered_motor2 = LowPassFilter_motor2.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
         Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
@@ -313,7 +336,7 @@
     
     void motor1_controller(void)
     {
-        error1_motor1 = (Yref_motor1 - positie_motor1);
+        error1_motor1 = (theta_motor1 - positie_motor1);
         if (motor1_calibrated==true&&motor2_calibrated==true)
             {
                 motor1_pwm();
@@ -323,13 +346,55 @@
 
     void motor2_controller(void)
     {
-        error1_motor2 = (Yref_motor2 - positie_motor2);
+        error1_motor2 = (theta_motor2 - positie_motor2);
         if (motor1_calibrated==true&&motor2_calibrated==true)
             {
                 motor2_pwm();
             } 
     }   
     
+    double Angle_motor1()
+    {
+        a = atan(y / x);
+        b = asin((length_link2 * sin(theta_motor2)) / (sqrt(pow(x, 2.0) + pow(y, 2.0))));
+        theta_motor1 = b + a;
+        return theta_motor1;
+    }
+
+    double Angle_motor2()
+    {
+        c = (pow(x, 2.0) + pow(y, 2.0) - pow(length_link1, 2.0) - pow(length_link2, 2.0));
+        d = (2 * length_link1 * length_link2);
+        theta_motor2 = acos(c / d);
+        return theta_motor2;
+    }
+    
+    double RKI()
+    {
+        if (emg0_active==true)
+        { if (x>x_max) {x=x_max;}
+          else { x=x+ x_vergroting; } 
+        }
+        
+        if (emg1_active==true)
+        { if (y>y_max) {y=y_max;}
+          else {y=y + y_vergroting; }
+         }
+        
+        if (emg2_active==true)
+        { if (x<x_min) {x=x_min;}
+          else {x = x - x_vergroting; } 
+        }
+        
+        if (emg3_active==true)
+        { if (y<y_min) {y=y_min;}
+         else {y=y - y_vergroting;} 
+        } 
+        
+        Angle_motor1();
+        Angle_motor2();
+    } 
+     
 void emg0_processing()
 {
     emg0_raw_signal=emg0.read();
@@ -433,11 +498,6 @@
                 if(button2==0) {State=Homing;}
             }
             
-            
-            
-//            Yref_motor1=5000;
-//            Yref_motor2=2000;
-//            State=Homing; 
             break; 
         case Homing: 
        //     pc.printf("\r\n State: Homing");
@@ -457,13 +517,10 @@
             break; 
   
         case Operating:
-  /*          pc.printf("\r\n State: Operating");
+//          pc.printf("\r\n State: Operating");
             led_blue.write(1);
             led_red.write(1);
             led_green.write(0);
-            wait(0.5);
-            led_green.write(1);
-            wait(0.5); */
             break; 
         
         case Demo:
@@ -529,6 +586,11 @@
 // 5.2 Run state-machine(s) ****************************************************
 state_machine() ;
 // 5.3 Run controller(s) *******************************************************
+motor1_calibrated=true;motor2_calibrated=true;
+if (button1==0){emg0_active=true;}else {emg0_active=false;} //emg1_active=true;} else {emg0_active=false;emg1_active=false;}
+if (button2==0){emg2_active=true;}else {emg2_active=false;} //emg3_active=true;} else {emg2_active=false;emg3_active=false;}
+RKI();
+
 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
 PID_controller_motor2(error_integral_motor2, error1_prev_motor2);
 // 5.4 Send output signals to digital and PWM output pins **********************
@@ -555,7 +617,7 @@
     led_green.write(1);
     led_red.write(1);
     led_blue.write(1);
-    State = EMGCalibration ; // veranderen naar MotorCalibration;
+    State = Operating ; // veranderen naar MotorCalibration;
     ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second