Diff: main.cpp

Revision:

0:0cd34994e6d6

--- /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