working PID + Kinematics + Motorcontrol

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Revision:
8:cfe7ad86837c
Parent:
7:83a69ca630bc
Child:
9:59dc4c12e4ee
--- a/main.cpp	Wed Oct 31 20:03:47 2018 +0000
+++ b/main.cpp	Wed Oct 31 23:05:26 2018 +0000
@@ -79,11 +79,11 @@
 int EMGymin ;
 
 // Dit moet experimenteel geperfectioneerd worden
-float tijdstap = 0.05;      //nu wss heel langzaam, kan miss omhoog
+float tijdstap = 0.005;      //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE
 float v = 0.1;                // snelheid kan wss ook hoger
 
-float px = 0.2;     //starting x    
-float py = 0.155;   // starting y
+float px = 0.2;     //starting x    // BOUNDARIES
+float py = 0.155;   // starting y   // BOUNDARIES
 
 // verschil horizontale as met de actieve arm
 float da1 = 1.619685; // verschil a1 hoek en motor
@@ -91,10 +91,10 @@
 float da3 = 3.372859;
 
 // limits (since no forward kinematics)
-float upperxlim = 0.36; //niet helemaal naar requierments ff kijken of ie groter kan
-float lowerxlim = 0.04;
-float upperylim = 0.30;
-float lowerylim = 0.04;
+float upperxlim = 0.25; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
+float lowerxlim = 0.15;
+float upperylim = 0.20;
+float lowerylim = 0.10;
 
 
 //----------------FUNCTIONS--------------------------
@@ -244,7 +244,7 @@
 void Motor_mover()
 {
     double motor_position = encoder1.getPulses(); //output in counts
-    double reference_rotation = hoek2(px, py);
+    double reference_rotation = hoek1(px, py);
     double error = reference_rotation - motor_position*(2*PI)/8400;
     double u = PID_controller(error);
     moter_control(u);
@@ -256,7 +256,7 @@
     moter2_control(u_2);
     
     double motor_position3 = encoder3.getPulses(); //output in counts
-    double reference_rotation3 = hoek2(px, py);
+    double reference_rotation3 = hoek3(px, py);
     double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
     double u_3 = PID_controller(error_3);
     moter3_control(u_3);  
@@ -291,7 +291,7 @@
         
         // Tickers
         //show_counts.attach(PrintFlag, 0.2);
-        ref_rot.attach(Motor_mover, 0.01);
+        ref_rot.attach(Motor_mover, 0.001);
         //Scope_Data.attach(ScopeData, 0.01);
         
 
@@ -300,7 +300,7 @@
         
 if (button2 == false)
 {
-   wait(0.05f);
+   wait(0.01f);
         
         // berekenen positie
            float px = positionx(1,0);  // EMG: +x, -x
@@ -309,7 +309,7 @@
                 }        
 
 if (button1 == false){
-    wait(0.05f);
+    wait(0.01f);
             // berekenen positie
            float px = positionx(0,1);  // EMG: +x, -x
             float py = positiony(0,1);  // EMG: +y, -y