Revision 4:6524b198721f, committed 2016-10-13

Comitter:

MBroek

Date:

Thu Oct 13 18:34:24 2016 +0000

Parent:

3:581c5918b590

Child:

5:c510ab61af28

Commit message:

Test Opstelling voor acyual position test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Thu Oct 13 18:34:24 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#188304906687

--- a/main.cpp	Thu Oct 13 15:05:35 2016 +0000
+++ b/main.cpp	Thu Oct 13 18:34:24 2016 +0000
@@ -10,7 +10,7 @@
 #include "HIDScope.h"
 
 // Definieren van de HIDscope ----------------------------------------
-HIDScope scope(1)               
+HIDScope scope(1);               
 
 
 // Definieren van de Serial en Encoder -------------------------------
@@ -36,26 +36,39 @@
 AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
 
 
+//Definieren van de tickers ------------------------------------------
+Ticker test_ticker;
+Ticker hidscope_ticker;
+
+
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
+
 volatile bool safe = true;                      // Conditie voor de while-loop in main
 
-int position_motor1;
-int counts_motor1;
+double position_motor1;
+
+int counts1;                                // Pulses van motoren
+int counts2;
 
 const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
-const double rad_per_count = pi/64.0;       // Aantal rad per puls uit encoder
+const double rad_per_count = pi/8400.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;
 
-double error1_int = 0.d;                 // Initiele error integral waardes
-double error2_int = 0.d;
+double error1_int = 0.00000;                 // Initiele error integral waardes
+double error2_int = 0.00000;
 
 const double T_getpos = 0.01;           // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
 
+volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
+
+const double Kp = 1.0000000;                    // De PID variablen
+const double Kd = 1.0000000;
+const double Ki = 1.0000000;
+
 
 
 
@@ -85,7 +98,7 @@
 
 // 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)
+    double 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
 }
@@ -99,13 +112,13 @@
 
 
 // De PID-controller voor de motors -------------------------------------------------
-double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int){
+double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int, const double Kp, const double Kd, const double Ki){
     double error = ref_pos - mot_pos;       // error uitrekenen
-    double error_dif = (error-error_prev)/T_getpos      // De error differentieren
+    double error_dif = (error-error_prev)/T_getpos;      // De error differentieren
     //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
+    return Kp*error + Ki*error_int + Kd*error_dif;       // De uiteindelijke PID output
 }
        
 
@@ -139,6 +152,11 @@
 }
 
 
+// De go-flag functies -----------------------------------------------------------------
+void flag1_activate(){flag1=true;}           // Activeert de flag
+void flag2_activate(){flag2=true;}    
+
+
 
 // DE MAIN =================================================================================================================
 int main()
@@ -146,10 +164,22 @@
     pc.baud(SERIALBAUD);
     pc.printf("Starting");
     
-    test_button.fall(&PID_controller);      // Activeert test button
+    //test_button.fall(&);      // Activeert test button
     kill_switch.fall(&emergency_stop);      // Activeert kill switch
     
+    test_ticker.attach(&flag1_activate,0.1);                  // Activeert de go-flag van motor positie 
+    hidscope_ticker.attach(&flag2_activate,0.01);     
+    
     while(safe){            // Draait loop zolang alles veilig is.
+        if (flag1){
+            flag1 = false;
+            position_motor1 = get_position_m1(rad_per_count);
+            pc.printf("%f\r\n",position_motor1);
+        }
+        if (flag2){
+            flag2 = false;
+            set_scope(position_motor1);
+        }
     }
     
     motor1_speed_pin = 0;   //Dit zet de motoren uit nadat de kill switch is gebruikt

--- a/mbed.bld	Thu Oct 13 15:05:35 2016 +0000
+++ b/mbed.bld	Thu Oct 13 18:34:24 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file