In deze code vinden jullie een PIDregelaar en begrenzer!

Dependencies:   Encoder HIDScope mbed

Files at this revision

API Documentation at this revision

Comitter:
phgbartels
Date:
Wed Oct 29 11:13:23 2014 +0000
Commit message:
PIDregelaar_begrenzer

Changed in this revision

Encoder.lib Show annotated file Show diff for this revision Revisions of this file
HIDScope.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 16ee9fc421f8 Encoder.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Wed Oct 29 11:13:23 2014 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/vsluiter/code/Encoder/#18b000b443af
diff -r 000000000000 -r 16ee9fc421f8 HIDScope.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Wed Oct 29 11:13:23 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#e44574634162
diff -r 000000000000 -r 16ee9fc421f8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 29 11:13:23 2014 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+#include "PwmOut.h"
+/*definieren pinnen Motor 1*/
+#define M1_PWM PTA5
+#define M1_DIR PTA4
+#define M2_PWM PTC8
+#define M2_DIR PTC9
+/*Definieren minimale waarden PWM per motor*/
+#define PWM1_min_50 0.529 /*met koppelstuk!*/
+#define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
+/*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
+#define TSAMP 0.005
+#define K_P (100)
+#define K_I (50 * TSAMP)
+#define K_D (1)
+//#define K_D (0.0005 /TSAMP)
+#define I_LIMIT 100.
+#define PI 3.1415926535897
+#define lengte_arm 0.5
+ 
+Encoder motor1(PTD3,PTD1, true);
+PwmOut pwm_motor1(M1_PWM);
+DigitalOut motordir1(M1_DIR);
+void clamp(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+volatile bool looptimerflag;
+float PWM1_percentage = 0;
+int PWM2_percentage = 100; 
+int aantal_rotaties_1 = 10;
+int aantalcr_1 = 1600;
+int aantalcr_2 = 960; 
+int beginpos_motor1;
+int beginpos_motor1_new; 
+int beginpos_motor2;
+int beginpos_motor2_new;
+uint16_t gewenste_snelheid = 2;
+uint16_t gewenste_snelheid_rad = 4; 
+int previous_pos_motor1 = 0;
+int current_pos_motor1;
+float pos_motor1_rad;
+int delta_pos_motor1_puls;
+float delta_pos_motor1_rad;
+float snelheid_motor1_rad;
+float snelheid_arm_ms; 
+float PWM1; 
+float Speed_motor1;
+float Speed_motor1rad;
+ 
+void setlooptimerflag(void) //Ticker maakt een constructie die zoveel keer per seconde een functie uitvoerd,  met sampingling freq TSAMP
+{
+    looptimerflag = true;
+}
+    
+int main()
+{
+    motor1.setPosition(0);
+    pwm_motor1.period_us(100);
+    float setpoint;
+    float prev_setpoint = 0; 
+    Ticker looptimer; 
+    looptimer.attach(setlooptimerflag,TSAMP);   
+    
+    while(true)
+    {
+            while(!looptimerflag) //Hierdoor zeg je eigenlijk tegen je systeem; voer dit script uit na elke TSAMP s. 
+            {
+            /*do nothing*/  //Zolang dit waar is, doe je niets. 
+            }
+        looptimerflag = false; //clear flag
+                
+        current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
+        pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
+        
+        //begrenzen van de hoek(in dit geval 90 graden)
+        if (current_pos_motor1 >= 400)
+        gewenste_snelheid_rad = 0; 
+        else
+        
+        setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;       
+        PWM1_percentage = pid(setpoint, pos_motor1_rad);
+       
+// zorgen dat PWM1_percentage niet buiten de range van 100% valt       
+        if (PWM1_percentage < -100)
+            PWM1_percentage = -100;
+        else if (PWM1_percentage >100)
+            PWM1_percentage =100;
+        else
+        
+        // bepalen van de richting
+        if(PWM1_percentage < 0)
+            motordir1 = 1;
+        else
+            motordir1 = 0;
+            // ennnn uitsturen van PWM 
+        pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+        scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+        prev_setpoint = setpoint;       
+    }    
+}
+
+void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op
+// maar de locatie van de variabele. 
+{
+    *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
+    // *in = het getal dat staat op locatie van in --> waarde van new_pwm
+}
+
+
+float pid(float setpoint, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    static float    out_i = 0;
+    float           out_d = 0;
+    error  = (setpoint-measurement);
+    out_p  = error*K_P; 
+    out_i += error*K_I;
+    out_d  = (error-prev_error)*K_D;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    prev_error = error;
+    return out_p + out_i + out_d;
+}
\ No newline at end of file
diff -r 000000000000 -r 16ee9fc421f8 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 29 11:13:23 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file