Diff: main.cpp

Revision:

1:5b3fa8e47e8b

Parent:

0:9e053ed05c69

Child:

2:6bef5397e8a9

--- a/main.cpp	Tue Oct 11 10:21:37 2016 +0000
+++ b/main.cpp	Thu Oct 13 09:23:04 2016 +0000
@@ -28,6 +28,8 @@
 //Definieren van bord-elementen --------------------------------------
 InterruptIn kill_switch(D7);
 InterruptIn test_button(D6);
+AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
+AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
 
 
 
@@ -40,6 +42,8 @@
 const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
 const double rad_per_count = pi/64.0;       // Aantal rad per puls uit encoder
 
+const double meter_per_count = 1;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
+
 double error_prev = 0.00000;            // Initiele error derivative waardes
 double error_prev = 0.00000;
 
@@ -60,19 +64,35 @@
     pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
 }
 
- 
-double get_positions(){
+
+// Functie voor het vinden van de positie van motor 1 ---------------------
+double get_position_m1(const double radpercount){          //returned de positie van de motor in radialen (positie vd arm in radialen)
     counts1 = encoder_motor1.getPulses();           // Leest aantal pulses uit encoder af
-    motor1_position = rad_per_count * counts1;      // rekent positie motor1 uit
-    reference1_position = ....              // = constante * emg signaal 1+2
+    return radpercount * counts1;      // rekent positie motor1 uit en geeft deze als output vd functie
+}
+
+
+// Functie voor vinden van de positie van motor 2 -----------------------------
+double get_position_m2(const double meterpercount){     // returned de positie van het karretje in meter
+    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
+}
+
+
+// Functie voor het vinden van de reference position van motor 1 en 2 --------------------
+double get_reference_position_m1(double aantal_rad, double aantal_meter){
+    ref_pos = pot1.read - pot2.read;     // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
+    return ref_pos * aantal_rad;            // Aantal rad is hoeveelheid radialen van middelpunt tot minima
+    return ref_pos * aantal_meter           // Aantal meter is de helft van de lengte die het karretje kan bewegen
+}
    
  
 
-// De PID-controller voor motor 1 -------------------------------------------------
-double PID_controller1(double ref_pos, double mot_pos, double &error_prev, double &error_int){
+// De PID-controller voor de motors -------------------------------------------------
+double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int){
     double error = ref_pos - mot_pos;       // error uitrekenen
     double error_dif = (error-error_prev)/T_getpos      // De error differentieren
-    error_dif = .....       // Filter biquad poep
+    //error_dif = .....       // Filter biquad poep
     error_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
     error_int = error_int + T_getpos*error;             // De error integreren
     return Kp*error + Ki*error_int + Kd*error_dif       // De uiteindelijke PID output