Diff: main.cpp

Revision:

0:16ee9fc421f8

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