Diff: main.cpp

Revision:

28:94da9cdc1f16

Parent:

27:f5b33cbd3c22

Child:

29:55547603475e

--- a/main.cpp	Thu Oct 31 18:54:29 2019 +0000
+++ b/main.cpp	Fri Nov 01 14:07:32 2019 +0000
@@ -44,26 +44,7 @@
 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 = 0;   // (of -2?) 8.5 < x < 30.5
-    double x_max= 100; //30.5;
-    double x_min= -100; //8.5;
-    double x_home=8.5;
-    double x_vergroting=0.02;
-    
-    double y = 0; // 7.5 < y < 27
-    double y_max=100; //27;
-    double y_min=-100; //7.5;
-    double y_home=7.5;
-    double y_vergroting=0.02;
-    double a;
-    double b;
-    double c;
-    double d;
+
     
     bool emg0_active;
     bool emg1_active;
@@ -136,7 +117,7 @@
     int demo_tijd=0;
     
 // 3.5 Motor 1 variables ***************************************************************   
-    //3.5a PID-controller motor 1
+    
     double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
     static double error_integral_motor1 = 0;
     double Yref_motor1;                                 
@@ -158,7 +139,7 @@
     double positie_prev_motor1;
 
 // 3.5 Motor 2 variables ***************************************************************
-    //3.5b PID-controller motor 2
+    
     double counts_per_rad_motor2 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
     static double error_integral_motor2 = 0;
     double Yref_motor2;                                 
@@ -178,6 +159,29 @@
     
     double positie_verschil_motor2;
     double positie_prev_motor2;    
+    
+// 3.6 RKI Variables ***********************************************************    
+    
+    double theta_motor1;  //Hoek motor 1
+    double theta_motor2;  //Hoek motor2
+    double pi=3.14159265358979323846; 
+    double length_link1=16.25; 
+    double length_link2=15;
+    double x = 0;   // X positie in het  begin, workspace max: 8.5 < x < 30.5
+    double x_max= 100; //Maximale workspace waarde X, zou 30.5 moeten zijn
+    double x_min= -100; //Minimale workspace waarde X, zou 8.5 moeten zijn;
+    double x_home=3.0;  //Home positie, waar de x heen gaat bij homing
+    double x_vergroting=0.02; //De stap waarmee de x groter wordt bij elke keer als de emg actief is. EMG wordt elke 1 miliseconde gerund. Dus als je 1 seconde lang een emg signaal geeft, krijg je een vergroting van 100.
+    
+    double y = 0; // Y positie in het  begin, workspace max:7.5 < y < 27
+    double y_max=100; //Minimale workspace waarde y, zou 27 moeten zijn
+    double y_min=-100; // Minimale workspace waarde y, zou 7.5 moeten zijn;
+    double y_home=0; //Home positie, waar de y heen gaat bij homing
+    double y_vergroting=0.02; //De stap waarmee de y groter wordt bij elke keer als de emg actief is. EMG wordt elke 1 miliseconde gerund. Dus als je 1 seconde lang een emg signaal geeft, krijg je een vergroting van 100.
+    double a; //Constante voor de deel van de formules voor RKI
+    double b; //Constante voor de deel van de formules voor RKI
+    double c; //Constante voor de deel van de formules voor RKI
+    double d; //Constante voor de deel van de formules voor RKI
 
 //******************************************************************************
 // 4. Functions ****************************************************************
@@ -198,28 +202,29 @@
         scope.send();   
     }
     
-    // 4.x Encoder motor1 ****************************************************************
+    // 4.2a Encoder motor1 ****************************************************************
         double fencoder_motor1()                                    // bepaalt de positie van de motor
         {
             positie_motor1 = encoder_motor1.getPulses()/counts_per_rad_motor1;                // haalt encoder waardes op
-            positie_verschil_motor1 =  (positie_motor1-positie_prev_motor1)/Ts;
+            positie_verschil_motor1 =  (positie_motor1-positie_prev_motor1)/Ts; //voor de motorcalibratie
             positie_prev_motor1 = positie_motor1;
             
             return positie_motor1;                                 //geeft positie van motor
         }
-    // 4.x Encoder motor2 ****************************************************************        
+    // 4.2b Encoder motor2 ****************************************************************        
         double fencoder_motor2()                                    // bepaalt de positie van de motor
         {
             positie_motor2 = encoder_motor2.getPulses()/counts_per_rad_motor2;                // haalt encoder waardes op
-            positie_verschil_motor2 =  (positie_motor2-positie_prev_motor2)/Ts;
+            positie_verschil_motor2 =  (positie_motor2-positie_prev_motor2)/Ts;   //voor de motorcalibratie
             positie_prev_motor2 = positie_motor2;
             
             return positie_motor2;                                 //geeft positie van motor
         }
         
-    // 4.xa Calibration motors
+    // 4.3 Calibration motors ****************************************************************
         
-    void motor_calibration()
+        // 4.3a Calibration motors ****************************************************************     
+    void motor_calibration() 
     {
         // Calibration motor 1
         motor1DirectionPin=0; //direction of the motor
@@ -235,7 +240,7 @@
      //   pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
     
     
-    
+    // 4.3b Calibration motors ********************************************
         // Calibration motor 2
         motor2DirectionPin=0; //direction of the motor
         motor2=1.0;
@@ -254,7 +259,7 @@
             
     }
     
-    // 4.2a PID-Controller motor 1**************************************************
+    // 4.4a PID-Controller motor 1**************************************************
     double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
     {
         //Proportional part
@@ -278,7 +283,7 @@
         return P_motor1; 
     }
 
-    // 4.2b PID-Controller motor 2**************************************************
+    // 4.4b PID-Controller motor 2**************************************************
     double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
     {
         //Proportional part
@@ -301,7 +306,7 @@
 
         return P_motor2; 
     }
-    
+   // 4.5a Motor 1 direction******************************************** 
     double motor1_pwm()
     {
         
@@ -323,6 +328,8 @@
             motor1 = fabs(P_motor1);
         }
     }  
+    
+    // 4.5b Motor 1 direction ********************************************
    double motor2_pwm()
     {
         
@@ -344,6 +351,29 @@
             motor2 = fabs(P_motor2);
         }            
     }
+    
+    // 4.6a Motor 1 controller ******************************************** 
+    void motor1_controller(void)
+    {
+        error1_motor1 = (theta_motor1 - positie_motor1);
+         
+        if (motor1_calibrated==true&&motor2_calibrated==true)
+            {
+               motor1_pwm();
+            } 
+        
+    }
+
+    // 4.3a Motor 2 controller ********************************************
+    void motor2_controller(void)
+    {
+        error1_motor2 = (theta_motor2 - positie_motor2);
+        if (motor1_calibrated==true&&motor2_calibrated==true)
+            {
+                motor2_pwm();
+            } 
+    } 
+    
 void emg0_processing()
 {
     emg0_raw_signal=emg0.read();
@@ -460,25 +490,7 @@
         Angle_motor2();
     } 
 
-    void motor1_controller(void)
-    {
-        error1_motor1 = (theta_motor1 - positie_motor1);
-         
-        if (motor1_calibrated==true&&motor2_calibrated==true)
-            {
-               motor1_pwm();
-            } 
-        
-    }
-
-    void motor2_controller(void)
-    {
-        error1_motor2 = (theta_motor2 - positie_motor2);
-        if (motor1_calibrated==true&&motor2_calibrated==true)
-            {
-                motor2_pwm();
-            } 
-    }      
+     
     
     // 4.3 State-Machine *******************************************************
 
@@ -496,8 +508,7 @@
             if (motor1_calibrated==true&&motor2_calibrated==true)
             {
                 pc.printf("\r\n Motor Calibration is done!");
-               // x=28; aangeven wat de huidige positie is 
-               // y=11;
+
                 encoder_motor1.reset();
                 encoder_motor2.reset();
 
@@ -554,7 +565,7 @@
             else if (6000<emg_tijd)
             {led_green.write(1);
             
-               calibration_done=true; // later verplaatsen
+               
                 if(button1==0) {State=StartWait;}
                 if(button2==0) {State=Homing;}
             }
@@ -581,6 +592,7 @@
   
         case Operating:
 //          pc.printf("\r\n State: Operating");
+            calibration_done=true;
             led_blue.write(1);
             led_red.write(1);
             led_green.write(0);
@@ -600,6 +612,7 @@
      //       emg2_threshhold=100;
      //       emg3_threshhold=100;
             
+            calibration_done=false;
             
             if (button1==0)
             {    led1=1;
@@ -673,14 +686,17 @@
 
 fencoder_motor1() ;
 fencoder_motor2() ;
+
 // 5.2 Run state-machine(s) ****************************************************
 state_machine() ;
+
 // 5.3 Run controller(s) *******************************************************
 
 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 **********************
 motor1_controller();
 motor2_controller();