Diff: main.cpp

Revision:

18:d3a7f8b4cb22

Parent:

17:c694a0e63a89

Child:

19:38c9d177b6ee

--- a/main.cpp	Wed Oct 29 11:51:32 2014 +0000
+++ b/main.cpp	Thu Oct 30 11:35:04 2014 +0000
@@ -6,40 +6,41 @@
 
 //Constanten definiëren en waarde geven
 #define SAMPLETIME_REGELAAR         0.005       //Sampletijd ticker regelaar motor
-#define KP_arm1                     0.01        //Factor proprotionele regelaar arm 1
-#define KI_arm1                     0           //Factor integraal regelaar arm 1
-#define KD_arm1                     0           //Factor afgeleide regelaar arm 1
-#define KP_arm2                     0.01        //Factor proprotionele regelaar arm 2
-#define KI_arm2                     0           //Factor integraal regelaar arm 2
-#define KD_arm2                     0           //Factor afgeleide regelaar arm 2
-#define SAMPLETIME_EMG              0.005       //Sampletijd ticker EMG meten
+#define KP_arm1                             //Factor proprotionele regelaar arm 1
+#define KI_arm1                     0         //Factor integraal regelaar arm 1
+#define KD_arm1                     0          //Factor afgeleide regelaar arm 1
+#define KP_arm2                     4        //Factor proprotionele regelaar arm 2
+#define KI_arm2                     0.5           //Factor integraal regelaar arm 2
+#define KD_arm2                     0.5           //Factor afgeleide regelaar arm 2
+#define SAMPLETIME_EMG              0       //Sampletijd ticker EMG meten
 #define PULS_ARM1_HOME_KALIBRATIE   180         //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
 #define PULS_ARM2_HOME_KALIBRATIE   393         //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
 
 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
 //High Pass filter
 #define A1 1
-#define A2 -1.5610
-#define A3 0.6414
-#define B1 0.0201
-#define B2 0.0402
-#define B3 0.0201
+#define A2 -1.561018075800718
+#define A3 0.641351538057563
+#define B1 0.800592403464570
+#define B2 -1.601184806929141
+#define B3 0.800592403464570
 
 //Notch filter
 #define C1 1
-#define C2 -1.1873E-16
-#define C3 0.9391
-#define D1 0.9695
-#define D2 -1.1873E-16
-#define D3 0.9695
+#define C2 -1.18733334554802E-16
+#define C3 0.939062505817492
+#define D1 0.969531252908746
+#define D2 -1.18733334554802E-16
+#define D3 0.969531252908746
 
 //Low pass filter
 #define E1 1
-#define E2 -1.9645
-#define E3 0.9651
-#define F1 1.5515E-4
-#define F2 3.1030E-4
-#define F3 1.5515E-4
+#define E2 -1.77863177782459
+#define E3 0.800802646665708
+#define F1 0.00554271721028068
+#define F2 0.0110854344205614
+#define F3 0.00554271721028068
+
 
 //Pinverdeling en naamgeving variabelen
 TextLCD         lcd(PTD1, PTA12, PTB2, PTB3, PTC2, PTD3, TextLCD::LCD16x2);        //LCD scherm
@@ -78,6 +79,8 @@
 float xb;                               //Gemeten EMG waarde biceps
 float xt;                               //Gemeten EMG waarde triceps
 
+float puls_arm1_home = 0;
+
 arm_biquad_casd_df1_inst_f32 highpass_biceps;               //Highpass_biceps IIR filter in direct form 1
 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3};     //Filtercoëfficiënten van het filter
 float highpass_biceps_states[8];                            //Aantal states van het filter, het aantal y(n-x) en x(n-x)
@@ -141,7 +144,8 @@
 
 void arm1_naar_thuispositie()           //Brengt arm 1 naar de beginpositie
 {
-    error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition());                //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+    puls_arm1_home = puls_arm1_home + 180/200;
+    error_arm1_kalibratie = (puls_arm1_home - puls_motor_arm1.getPosition());                //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
     integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR;                        //Integraal deel van regelaar
     afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR;                   //Afgeleide deel van regelaar
     pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
@@ -155,11 +159,13 @@
     {                                            
         dir_motor_arm1.write(0);
     }
+
+    pwm_to_motor_arm1=pwm_to_motor_arm1/20;
     pwm_motor_arm1.write(fabs(pwm_to_motor_arm1));      //Output PWM naar motor is de absolute waarde van de berekende PWM
     pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());       //Huidig aantal getelde pulsen van de encoder naar pc sturen
     pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);                            //Huidige PWM waarde naar motor naar pc sturen
 
-    if (pwm_to_motor_arm1 == 0)                         //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
+    if (error_arm1_kalibratie == 0)                         //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
     {                       
         state = KALIBRATIE_ARM2;                        //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
         pc.printf("KALIBRATIE_ARM1 afgerond\n");        //Tekst voor controle Arm 1 naar thuispositie