Diff: main.cpp

Revision:

23:3902ee714281

Parent:

22:6dfe5554b96e

Child:

24:ea6de9237cff

Child:

26:2726db5acc03

--- a/main.cpp	Tue Oct 25 12:03:43 2016 +0000
+++ b/main.cpp	Tue Oct 25 13:51:57 2016 +0000
@@ -64,9 +64,11 @@
 const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
 const double rad_per_count = 2.0*pi/8400.0;       // Aantal rad per puls uit encoder
 
-const double radius_tandwiel = 1.0;
+const double radius_tandwiel = 0.008;
 const double meter_per_count = rad_per_count * radius_tandwiel;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
 
+const double v_max_karretje = 8.4*radius_tandwiel;                  // Maximale snelheid karretje, gelimiteerd door de v_max vd motor
+
 double error1_int = 0.00000;                 // Initiele error integral waardes
 double error2_int = 0.00000;
 
@@ -78,15 +80,18 @@
 const double Kd_1 = 0.0;
 const double Ki_1 = 0.0;
 
-const double Kp_2 = 1.0000000;                    // De PID variablen voor motor 2
-const double Kd_2 = 0.1;
-const double Ki_2 = 0.3;
+const double Kp_2 = 1.0;                    // De PID variablen voor motor 2
+const double Kd_2 = 0.0;
+const double Ki_2 = 0.0;
 
 const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
-const double vrijheid_m2_meter = 0.25;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
+const double vrijheid_m2_meter = 0.5;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
 
 const double initial_pos_m1 = vrijheid_m1_rad;                // Startin posities van de motoren
-const double initial_pos_m2 = vrijheid_m2_meter;       
+const double initial_pos_m2 = 0; 
+
+double position_motor2; 
+double position_motor2_prev = initial_pos_m2;    
 
 double PID_output_1 = 0.0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
 double PID_output_2 = 0.0;
@@ -145,9 +150,13 @@
 
 
 // Functie voor vinden van de positie van motor 2 -----------------------------
-double get_position_m2(const double meterpercount){     // returned de positie van het karretje in meter
+double get_position_m2(const double meterpercount, double mot_pos_prev){     // returned de positie van het karretje in meter
+    double mot_pos;
     counts2 = encoder_motor2.getPulses();               // leest aantal pulses vd encoder af
-    return meterpercount * counts2;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
+    mot_pos = meterpercount * counts2 + mot_pos_prev;
+    encoder_motor2.reset();
+    mot_pos_prev = mot_pos;
+    return mot_pos_prev;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
 }
 
 
@@ -160,7 +169,7 @@
             ref_pos = ref_pos_prev;
             break;
         case 1 :
-            ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad;
+            ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad;         // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
             break;
         case -1 :
             ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad;
@@ -178,11 +187,30 @@
 
 
 // Functie voor het vinden van de reference position van motor 2 --------------------
-double get_reference_position_m2(const double aantal_meter){
-    double final_signal = EMG_sim1.read() - EMG_sim2.read();     // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
-    return final_signal * aantal_meter;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
+double get_reference_position_m2(double &ref_pos_prev, const double vrijheid_meter){
+    double ref_pos;
+    int final_signal = EMG_sim1.read() - EMG_sim2.read();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
+    switch(final_signal){
+        case 0 :
+            ref_pos = ref_pos_prev;
+            break;
+        case 1 :
+            ref_pos = ref_pos_prev + T_motor_function*v_max_karretje;           // Hierdoor veranderd de ref_pos met de maximale snelheid vd motor (karretje)
+            break;
+        case -1 :
+            ref_pos = ref_pos_prev - T_motor_function*v_max_karretje;
+            break;
+    }
+    if(ref_pos >= vrijheid_meter){
+        ref_pos = vrijheid_meter;
+    }
+    if(ref_pos <= 0){
+        ref_pos = 0;
+    }
+    ref_pos_prev = ref_pos;
+    return ref_pos_prev;                                                                                
 }
-   
+
 
 // HIDScope functie ----------------------------------------------------------------------
 void set_scope(double input1, double input2, double input3){
@@ -271,13 +299,16 @@
                     PID_output_2 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1 
                     set_motor1(PID_output_1);
                     set_motor2(PID_output_2);
+                    encoder_motor2.reset();
                     break;
                 case motor2 :
-                    reference_pos_m2 = get_reference_position_m2(vrijheid_m2_meter);
-                    PID_output_2 = PID_controller(reference_pos_m2, get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
+                    reference_pos_m2 = get_reference_position_m2(ref_pos_prev_m2, vrijheid_m2_meter);
+                    position_motor2 = get_position_m2(meter_per_count, position_motor2_prev);
+                    PID_output_2 = PID_controller(reference_pos_m2, position_motor2, error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
                     PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);   // Zorgt ervoor dat motor1 op de dezelfde positie blijft als motor 2 draait.
                     set_motor2(PID_output_2);
                     set_motor1(PID_output_1);
+                    position_motor2_prev = position_motor2;
                     break;
             }
     }
@@ -321,7 +352,7 @@
                 break;
             
         }
-        call_HIDscope(PID_output_1, reference_pos_m1, get_position_m1(meter_per_count));
+        call_HIDscope(PID_output_2, reference_pos_m2, position_motor2);
     }  
 }