Peter Bartels
In deze code vinden jullie een PIDregelaar en begrenzer!
Dependencies: Encoder HIDScope mbed
Revision 0:16ee9fc421f8, committed 2014-10-29
- Comitter:
- phgbartels
- Date:
- Wed Oct 29 11:13:23 2014 +0000
- Commit message:
- PIDregelaar_begrenzer
Changed in this revision
--- /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
--- /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
--- /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
--- /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