working PID + Kinematics + Motorcontrol

Revision:
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
```