Motors met een eigen wil
Dependencies: Encoder MODSERIAL mbed
Revision 4:836d7f9ac0ca, committed 2017-11-02
- Comitter:
- Annelotte
- Date:
- Thu Nov 02 11:29:50 2017 +0000
- Parent:
- 3:120fbef23c17
- Commit message:
- Onaf Miriam
;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 02 10:00:46 2017 +0000 +++ b/main.cpp Thu Nov 02 11:29:50 2017 +0000 @@ -24,12 +24,12 @@ float e_int2 = 0; double pi = 3.14159265359; -double SetPx = 38; //Setpoint position x-coordinate from changePosition (EMG dependent) -double SetPy = 30; //Setpoint position y-coordinate from changePosition (EMG dependent) +double SetPx = 0.38; //Setpoint position x-coordinate from changePosition (EMG dependent) +double SetPy = 0.30; //Setpoint position y-coordinate from changePosition (EMG dependent) double q1 = 0; //Reference position q1 from calibration (only the first time) double q2 = (pi/2); //Reference position q2 from calibration (only the first time) -const double L1 = 30; //Length arm 1 -const double L2 = 38; //Length arm 2 +const double L1 = 0.30; //Length arm 1 +const double L2 = 0.38; //Length arm 2 double K = 1; //Spring constant for movement end-joint to setpoint double B1 = 1; //Friction coefficient for motor 1 double B2 = 1; //Friction coefficient for motot 2 @@ -42,7 +42,7 @@ double cc; double a; double aa; - +bool autodemo_done = false; //automatische demo stand =0 //tweede motor AnalogIn potMeter1(A2); @@ -50,6 +50,7 @@ DigitalOut M2D(D4); Encoder motor2(D9,D8,true); + void RKI() { p=sin(q1)*L1; @@ -78,18 +79,18 @@ double Potmeterwaarde1 = potMeter1.read(); if (Potmeterwaarde2>0.6) { - SetPx++; // hoe veel verder gaat hij? 1 cm? 10 cm? + SetPx += 0.001; // hoe veel verder gaat hij? 1 cm? 10 cm? } else if (Potmeterwaarde2<0.4) { - SetPx--; + SetPx -= 0.001; } else {} if (Potmeterwaarde1>0.6) { - SetPy++; + SetPy += 0.001; } else if (Potmeterwaarde1<0.4) { - SetPy--; + SetPy -= 0.001; } else {} @@ -205,10 +206,34 @@ return Huidigepositie2; // huidige positie = current position } +void Autodemo_or_demo() +{ + if (autodemo_done == 0) + { + AutoSetpointRobotForward (); //verander de se + MeasureAndControl (); + AutoSetpointRobotHome (); + MeasureAndControl (); + AutoSetpointRobotDown (); + MeasureAndControl (); + AutoSetpointRobotHome (); + MeasureAndControl (); + autodemo_done = true; + } + + else if (autodemo_done == 1) + { + SetpointRobot(); + MeasureAndControl (); + } + +} + + + void MeasureAndControl(void) { - // RKI aanroepen - SetpointRobot(); + // RKI aanroepen RKI(); // hier the control of the control system @@ -230,7 +255,7 @@ int main() { M1E.period(PwmPeriod); - Treecko.attach(&MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende + Treecko.attach(&Autodemo_or_demo, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. pc.baud(115200);