working PID + Kinematics + Motorcontrol
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 8:cfe7ad86837c
- Parent:
- 7:83a69ca630bc
- Child:
- 9:59dc4c12e4ee
diff -r 83a69ca630bc -r cfe7ad86837c main.cpp --- 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