Diff: main.cpp

Revision:

8:83115293e84d

Parent:

7:2e4eb23700b0

Child:

9:80693874f9ce

--- a/main.cpp	Mon Nov 03 20:57:39 2014 +0000
+++ b/main.cpp	Mon Nov 03 21:13:38 2014 +0000
@@ -31,6 +31,8 @@
 #define KSLA_I (0.000001 *TSAMP1)
 #define KSLA_D (0.0005 /TSAMP1)
 
+#define MAXPOS 700
+
 // Constantes voor de filters definiëren:
 // Constantes voor de Low Pass filter
 #define A1LP    0.018180963222803
@@ -94,6 +96,30 @@
 // Teller voor hoeveel metingen er zijn gedaan
 uint16_t teller=0;
 
+//Definieer alle functies
+void clamp(float * in, float min, float max);
+void clampint(int * in, int min, int max);
+void setlooptimerflag(void);
+float readEMG1();
+float readEMG2();
+float notchfilter1(float ylp1);
+float notchfilter2(float ylp2);
+float hpfilter1(float yn1);
+float hpfilter2(float yn2);
+float lpfilter1(float yhp1);
+float lpfilter2(float yhp2);
+float filter1(float emg_value1);
+float filter2(float emg_value2);
+float threshold1 (float yave1);
+float threshold2 (float yave2);
+float getv(float delta_t);
+float resetarm();
+uint8_t badje (uint8_t y1, uint8_t y2);
+void batposition(int y);
+void sla(int k);
+float pidposition(float setpoint, float measurement);
+float pidarm(float rev_value, float mea_value);
+uint8_t armposition (uint8_t y1, uint8_t y2);
 
 /*FUNCTIES
 In deze sectie worden de functie geprogrammeerd, hieronder een uitleg per functie. De functies zijn verdeeld in drie groepen. In het script
@@ -364,6 +390,50 @@
 
 }
 
+void sla(int k)
+{   
+    float maxpwm;
+    float new_pwm;
+    switch(k)
+    {
+        case 1:
+            maxpwm=0.2;
+            break;
+        case 2:
+            maxpwm=0.5;
+            break;
+        case 3:
+            maxpwm=0.8;
+            break;
+        case 4:
+            maxpwm=1;
+            break;
+        default:
+            maxpwm=0;
+            break;
+    }
+    while((encoder1.getPosition()<MAXPOS)||getv(0.01)>0.1)
+    {
+        new_pwm=pidarm(MAXPOS,encoder1.getPosition());
+        clamp(&new_pwm,-maxpwm,maxpwm);
+        if (new_pwm>0)
+        {
+            dir1=1;
+        }
+        else if (new_pwm<0)
+        {
+            dir1=0;
+        }
+        pwm1.write(fabs(new_pwm));
+        cout<<"pwm1: "<<new_pwm<<endl;
+        cout<<"pos: "<<encoder1.getPosition()<<endl;
+    }
+      
+ pwm1.write(0);
+ wait(2);
+ //cout<< encoder1.getPosition()<<endl;
+}
+
 float pidposition(float setpoint, float measurement)
 {
     float error, out, out_p=0,out_d=0;
@@ -379,7 +449,21 @@
     return out;
 }
 
-
+float pidarm(float rev_value, float mea_value)
+{
+    float error;
+    static float prev_error = 0;
+    float p_out = 0;
+    static float i_out = 0;
+    float d_out = 0;
+    error = rev_value - mea_value;
+    p_out = error * KSLA_P;
+    i_out += error * KSLA_I;
+    d_out = (error - prev_error) * KSLA_D;
+    clamp(&i_out,-I_LIMIT1,I_LIMIT1);
+    prev_error=error;
+    return p_out + i_out + d_out;
+}
 
 
 uint8_t armposition (uint8_t y1, uint8_t y2)