Diff: main.cpp

Revision:

13:95a4bb9daf63

Parent:

12:3fec73bc3318

Child:

14:b1de410384c2

--- a/main.cpp	Mon Nov 03 15:15:31 2014 +0000
+++ b/main.cpp	Mon Nov 03 21:31:16 2014 +0000
@@ -26,6 +26,8 @@
 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
 #define TSAMP 0.005
 #define K_P (80000)
+#define K_P_motor2 (75)
+#define K_D_motor2 (0.01)
 #define K_I (0.01)
 #define K_D (0.01)
 #define I_LIMIT 100.
@@ -62,6 +64,7 @@
 int current_herhalingen_1 = 0;
 int previous_herhalingen_1 = 0;
 int previous_pos_motor1 = 0;
+int previous_pos_motor2 = 0;
 int current_pos_motor1;
 int current_pos_motor2;
 int EMG = 1;
@@ -69,14 +72,17 @@
 int doel;
 int doel_richting;
 int doel_hoogte;
-int puls_richting1;
+int puls_richting1 = 4000;
 int puls_richting2;
-int puls_richting3;
+int puls_richting3 = 4000;
 bool aanspan;
 void clamp(float * in, float min, float max);
 float pid(float setpoint, float measurement);
+float pid_motor2(float setpoint, float measurement);
 float pos_motor1_rad;
+float pos_motor2_rad;
 float PWM1_percentage = 0;
+float PWM2_percentage = 0;
 float PWM1;
 float PWM2;
 float setpoint = 0;
@@ -97,8 +103,9 @@
 float PWM_richting1;
 float PWM_richting2;
 float PWM_richting3;
+float position2_setpoint;
 
-enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
+enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES
 state_t state = RUST;
 
 void rust()
@@ -173,9 +180,7 @@
 {
     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
     motor1.setPosition(0);
-    motor2.setPosition(0);
-    pwm_motor1.period_us(100);
-    pwm_motor2.period_us(100);
+    //motor2.setPosition(0);
     akkoord_geven();
 }//void arm_kalibratie
 
@@ -214,68 +219,8 @@
     doel_bepaling();
 }//void meten_richting
 
-void instellen_richting()
-{
-    current_pos_motor2 = motor2.getPosition();
-    pc.printf("%d\n\r", current_pos_motor2);
-    /*
-    if (doel_richting ==1){
-    if (state==RETURN2RUST){
-        motordir2 = 1;
-        while (current_pos_motor2 > 0){
-            pwm_motor2.write(PWM_richting1);
-        }//while (current_pos_motor2 < rad_richting1)
-        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
-    }//if (doel_richting == 1)
-    else{
-    motordir2 = 0;
-        while (current_pos_motor2 < puls_richting1){
-            pwm_motor2.write(PWM_richting1);
-        }//while (current_pos_motor2 < rad_richting1)
-        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
-    }//else
-    }//if (doel_richting ==1)
-
-
-    else if (doel_richting == 2){
-    if (state==RETURN2RUST){
-        motordir2 = 1;
-        while (current_pos_motor2 > 0){
-            pwm_motor2.write(PWM_richting2);
-        }//while (current_pos_motor2 < rad_richting1)
-        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
-    }//if (doel_richting == 1)
-    else{
-        motordir2 = 0;
-        while (current_pos_motor2 < puls_richting2){
-            pwm_motor2.write(PWM_richting2);
-        }//while (current_pos_motor2 < rad_richting1)
-        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
-    }//if (doel_richting == 1)
-    }//if (doel_richting ==1)
-
-    else if(doel_richting == 3){
-    if (state==RETURN2RUST){
-        motordir2 =1;
-        while (current_pos_motor2 > 0){
-            pwm_motor2.write(PWM_richting3);
-        }//while (current_pos_motor2 < rad_richting1)
-        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
-    }//if (doel_richting == 1)
-    else{
-        motordir2 = 0;
-        while (current_pos_motor2 < puls_richting3){
-            pwm_motor2.write(PWM_richting3);
-        }//while (current_pos_motor2 < rad_richting1)
-        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
-    }//if (doel_richting == 1)
-    }//if (doel_richting ==1)
-    */
-}//void instellen_richting
-
 void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge)
 {
-
     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
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
@@ -301,10 +246,9 @@
 
     if (PWM1_percentage < -100) {
         PWM1_percentage = -100;
-    }//if
-    else if (PWM1_percentage >100) {
+    } else if (PWM1_percentage >100) {
         PWM1_percentage =100;
-    }//else if
+    }
 
     if(PWM1_percentage < 0) {
         motordir1 = 1;
@@ -333,6 +277,60 @@
     return out_p + out_i + out_d;
 }//float pid
 
+void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2)
+{
+    if (setpoint < position2_setpoint_rad) {
+        setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond);
+        if (setpoint > position2_setpoint_rad) {
+            setpoint = position2_setpoint_rad;
+        }
+    }
+
+    else if (setpoint > position2_setpoint_rad) {
+        setpoint = prev_setpoint - (0.0005 * speed2_radpersecond);
+        if (setpoint < position2_setpoint_rad) {
+            setpoint = position2_setpoint_rad;
+        }
+    } else if (setpoint == position2_setpoint_rad) {
+        current_herhalingen = previous_herhalingen + 1;
+        previous_herhalingen = current_herhalingen;
+    }
+
+    current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
+    pc.printf("c: %d\r\n", motor2.getPosition());//current_pos_motor2);
+    pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
+    PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad);
+
+    if (PWM2_percentage < -100) {
+        PWM2_percentage = -100;
+    } else if (PWM2_percentage >100) {
+        PWM2_percentage =100;
+    }
+
+    if(PWM2_percentage < 0) {
+        motordir2 = 1;
+    }//if
+    else {
+        motordir2 = 0;
+    }//else
+
+    pwm_motor2.write(abs(PWM2_percentage/100.));
+    prev_setpoint = setpoint;
+}
+
+float pid_motor2(float setpoint, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    float           out_d = 0;
+    error  = (setpoint-measurement);
+    out_p  = error*K_P_motor2;
+    out_d  = (error-prev_error)*K_D_motor2;
+    prev_error = error;
+    return out_p + out_d;
+}//float pid
+
 void clamp(float* in, float min, float max)
 {
 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
@@ -340,7 +338,6 @@
 
 void statemachinefunction()
 {
-    pc.printf("_%d_",statetimer.read_us());
     statetimer.reset();
     //pc.printf(".");
     switch(state) {
@@ -424,17 +421,31 @@
             break;
         }//case METEN_RICHTING
 
+
+        case TEST: {
+            state = RUST;
+            break;
+        }
+
         case INSTELLEN_RICHTING: {
-            instellen_richting();
-            
-            /*if (current_herhalingen == 100)
-            {
-                current_herhalingen_1 = 0;
-                previous_herhalingen_1 = 0;
+            //instellen_richting();
+            if (doel_richting == 1) {
+                position2_setpoint = 0;
+            } else if (doel_richting ==2) {
+                position2_setpoint = 14.5;
+            } else {
+                position2_setpoint = 25;
+            }
+
+
+            translatie(position2_setpoint, 10, 0.1);
+
+            if (current_herhalingen >= 400) {
+                current_herhalingen = 0;
+                previous_herhalingen = 0;
                 doel_richting = 0;
-                state = SLAAN;
-            }//if (current_herhalingen == 100)
-            */
+                state = RETURN2RUST;
+            }//if (current_herhalingen == 100
             break;
         }//case INSTELLEN_RICHTING
 
@@ -451,8 +462,8 @@
         }//case SLAAN
 
         case RETURN2RUST: {
-            instellen_richting();
-            GotoPosition(0,4, 0.05);
+            translatie(0,10,0.1);
+            /*GotoPosition(0,4, 0.05);
             doel_richting = 0;
             doel_hoogte = 0;
             if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) {
@@ -462,6 +473,7 @@
                 current_herhalingen = 0;
                 current_herhalingen = 0;
             }//if (current_herhalingen == 100)
+            */
             break;
         }// case RETURN2RUST
 
@@ -475,7 +487,6 @@
 
 void screenupdate()
 {
-    pc.printf("l");
     if(state==RUST) {
         lcd.cls();
         lcd.locate(0,0);
@@ -611,13 +622,11 @@
     }//else if(state==METEN_RICHTING){
 
     else if(state==INSTELLEN_RICHTING) {
-        pc.printf("i");
         lcd.cls();
         lcd.locate(0,0);
         lcd.printf("Instellen hoek");
         lcd.locate(0,1);
         lcd.printf("Even wachten...");
-        pc.printf("e");
     }//else if(state==INSTELLEN_RICHTING){
 
     else if(state==SLAAN) {
@@ -644,16 +653,18 @@
 
 int main()
 {
+    pwm_motor1.period_us(100);
+    pwm_motor2.period_us(100);
     pc.baud(115200);
     statetimer.start();
     arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
     arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
     arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
     arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
+    state = TEST;
     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
     //screen.attach(&screenupdate, 0.2);
-    while(1)
-    {
+    while(1) {
         screenupdate();
         wait(0.2);
     };