working PID + Kinematics + Motorcontrol
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 9:59dc4c12e4ee
- Parent:
- 8:cfe7ad86837c
diff -r cfe7ad86837c -r 59dc4c12e4ee main.cpp --- a/main.cpp Wed Oct 31 23:05:26 2018 +0000 +++ b/main.cpp Thu Nov 01 15:37:21 2018 +0000 @@ -19,6 +19,8 @@ // PID CONTROLLER -- PIN DEFENITIONS AnalogIn button2(A4); AnalogIn button1(A3); +DigitalIn button3(SW2); +DigitalIn button4(SW3); DigitalOut directionpin1(D7); // motor 1 DigitalOut directionpin2(D4); // motor 2 @@ -91,10 +93,10 @@ float da3 = 3.372859; // limits (since no forward kinematics) -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; +float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan +float lowerxlim = 0.10; +float upperylim = 0.225; +float lowerylim = 0.03; //0.03 is goed //----------------FUNCTIONS-------------------------- @@ -224,19 +226,38 @@ // DIRECTON AND SPEED CONTROL void moter_control(double u) { + directionpin1= u > 0.0f; //eithertrueor false + if (fabs(u)> 0.7f){ + u = 0.7f; + } + else { + u= u; + } pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } void moter2_control(double u) { directionpin2= u > 0.0f; //eithertrueor false + if (fabs(u)> 0.7f){ + u = 0.7f; + } + else { + u= u; + } pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } void moter3_control(double u) { directionpin3= u > 0.0f; //eithertrueor false + if (fabs(u)> 0.7f){ + u = 0.7f; + } + else { + u= u; + } pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } @@ -304,7 +325,7 @@ // berekenen positie float px = positionx(1,0); // EMG: +x, -x - float py = positiony(1,0); // EMG: +y, -y + float py = positiony(0,0); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); } @@ -312,10 +333,29 @@ wait(0.01f); // berekenen positie float px = positionx(0,1); // EMG: +x, -x + float py = positiony(0,0); // EMG: +y, -y + //printf("positie (%f,%f)\n\r",px,py); + } + +if (button3 == false){ + wait(0.01f); + // berekenen positie + float px = positionx(0,0); // EMG: +x, -x + float py = positiony(1,0); // EMG: +y, -y + //printf("positie (%f,%f)\n\r",px,py); + } + +if (button4 == false){ + wait(0.01f); + // berekenen positie + float px = positionx(0,0); // EMG: +x, -x float py = positiony(0,1); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); } + } + + // berekenen hoeken /* float a1 = hoek1(px, py);