Janine Klein Rot
Voor het testen van de motorsnelheid
Dependencies: Encoder MODSERIAL mbed
Revision 0:0cd34994e6d6, committed 2014-10-31
- Comitter:
- JKleinRot
- Date:
- Fri Oct 31 19:04:28 2014 +0000
- Commit message:
- Voor het testen van de motorsnelheid
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#180e968a751e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Oct 31 19:04:28 2014 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+#define SAMPLETIME_REGELAAR 0.005
+#define KP_arm2 0.05 //Factor proprotionele regelaar arm 2
+#define KI_arm2 0 //Factor integraal regelaar arm 2
+#define KD_arm2 0 //Factor afgeleide regelaar arm 2
+ #define A_TTTT 10049
+ float t = 0;
+
+PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
+DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
+Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
+
+Ticker ticker_motor_arm2_pid;
+Ticker ticker_regelaar;
+MODSERIAL pc(USBTX, USBRX);
+
+float pwm_to_motor_arm2; //PWM output naar motor arm 2
+float error_arm2_kalibratie; //Error in pulsen arm 2
+float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
+float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
+float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
+float referentie_arm2 = 0;
+
+
+volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
+void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
+{
+ regelaar_ticker_flag = true;
+}
+
+void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
+{
+ if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
+ *in = min;
+ }
+ if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
+ *in = max;
+ } else { //In alle andere gevallen is de waarde de waarde zelf
+ *in = *in;
+ }
+}
+
+void motor_arm2_pid()
+{
+ error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
+ vorige_error_arm2 = error_arm2_kalibratie;
+ keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+
+ if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
+ dir_motor_arm2.write(1);
+ } else { //Anders richting nul
+ dir_motor_arm2.write(0);
+ }
+ pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
+}
+
+int main()
+{
+ pc.printf("hoi");
+ ticker_motor_arm2_pid.attach(motor_arm2_pid, SAMPLETIME_REGELAAR);
+ ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
+
+ pc.baud(38400);
+
+ while(1) {
+
+
+ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ referentie_arm2 = 0.5 * 10049 * t * t;
+ t = t + 0.0165;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(referentie_arm2 >= 1902) {
+ while(pwm_to_motor_arm2 != 0) {
+ referentie_arm2 = referentie_arm2 - 90/200;
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+ if(pwm_to_motor_arm2 == 0) {
+ pc.printf("staat stil");
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Oct 31 19:04:28 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file