Script of MBR Group 20. Control of robot by EMG and/or potmeters
Dependencies: Encoder HIDScope MODSERIAL biquadFilter mbed
Fork of Script_Group_20 by
main.cpp
- Committer:
- Gerber
- Date:
- 2017-11-03
- Revision:
- 20:14edaecd7413
- Parent:
- 19:591572f4e4b5
- Child:
- 21:9307dc9be4f7
- Child:
- 24:847321a23e60
File content as of revision 20:14edaecd7413:
//libaries #include "mbed.h" #include "BiQuad.h" #include "HIDScope.h" #include "encoder.h" #include "MODSERIAL.h" //State Machine en calibratie enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo}; States State; int Counter; bool Position_controller_on; double looptime = 0.002f; double value; double home1; DigitalIn button (D1); //Encoder/motor double Huidigepositie1; double Huidigepositie2; double motorValue1; double motorValue2; //globalvariables Motor Ticker Treecko; //We make a awesome ticker for our control system Ticker printer; PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed PwmOut M2E(D5); DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control Encoder motor1(D13,D12,true); Encoder motor2(D9,D8,true); DigitalOut M2D(D4); //DEMO AnalogIn potMeter2(A1); AnalogIn potMeter1(A2); MODSERIAL pc(USBTX,USBRX); double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) const double Ts = 0.002f; // tickettijd/ sample time double e_prev = 0; double e_int = 0; double e_prev2 = 0; double e_int2 = 0; double tijdstap = 0.002; volatile double LBF; volatile double RBF; volatile double LTF; volatile double RTF; //buttons en leds voor calibration DigitalIn button1(PTA4); DigitalOut led(D2); //MVC for calibration double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0; //MEAN for calibration - rest double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0; double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ; double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT; bool caldone = false; int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample int Timescalibration = 0; int TimescalibrationREST = 0; // Biquad filters voor Left Bicep (LB) // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFLB; BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFLB; BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFLB; // Biquad filters voor Right Bicep (RB) // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFRB; BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFRB; BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFRB; // Biquad filters voor Left Tricep (LT) // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFLT; BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFLT; BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFLT; // Biquad filters voor Left Tricep (RT) // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFRT; BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFRT; BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFRT; Timer LooptimeEMG; //moetuiteindelijk weg //filters double emgNotchLB; double emgHPLB; double emgAbsHPLB; double emgLPLB; double emgNotchRB; double emgHPRB; double emgAbsHPRB; double emgLPRB; double emgNotchLT; double emgHPLT; double emgAbsHPLT; double emgLPLT; double emgNotchRT; double emgHPRT; double emgAbsHPRT; double emgLPRT; double f = 500; // frequency double dt = 1/f; // sample frequency AnalogIn emgLB(A0); // EMG lezen AnalogIn emgRB(A1); AnalogIn emgLT(A2); AnalogIn emgRT(A3); //double MVCLB = 0.3; //double MVCRB = 0.3; //double MVCLT = 0.3; //double MVCRT = 0.3; // variabelen changePosition int goalx, goaly; // variables RKI double pi = 3.14159265359; double q1 = (pi/2); //Reference position hoek 1 in radiance double q2 = -(pi/2); //Reference position hoek 2 in radiance const double L1 = 0.30; //Length arm 1 in mm const double L2 = 0.38; //Length arm 2 in mm double B1 = 1; //Friction constant motor 1 double B2 = 1; //Friction constant motor 2 double K = 1; //Spring constant movement from end-effector position to setpoint position double Tijd = 1; //Timestep value double Rsx = 0.38; //Reference x-component of the setpoint radius double Rsy = 0.30; //Reference y-component of the setpoint radius double refP = 0; //Reference position motor 1 double refP2 = 0.5*pi; //Reference position motor 2 double Rex = cos(q1)*L1 - sin(q2)*L2; //The x-component of the end-effector radius double Rey = sin(q1)*L1 + cos(q2)*L2; //The y-component of the end-effector radius double R1x = 0; //The x-component of the joint 1 radius double R1y = 0; //The y-component of the joint 1 radius double R2x = cos(q1)*L1; //The x-component of the joint 2 radius double R2y = sin(q1)*L1; //The y-component of the joint 1 radius double Fx = 0; double Fy = 0; double Tor1 = 0; double Tor2 = 0; double w1= 0; double w2= 0; void Filteren() { // LooptimeEMG.reset(); // LooptimeEMG.start(); //EMG 1 emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0. emgAbsHPLB = abs(emgHPLB); // Take absolute value emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope emgMEANSUBLB = emgLPLB - RESTMEANLB; //substract the restmean value LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1. emgNotchRB = NFRB.step(emgRB.read()); // Notch filter emgHPRB = HPFRB.step(emgNotchRB); // High-pass filter: also normalises around 0. emgAbsHPRB = abs(emgHPRB); // Take absolute value emgLPRB = LPFRB.step(emgAbsHPRB); // Low-pass filter: creates envelope emgMEANSUBLB = emgLPLB - RESTMEANLB; RBF = emgLPRB/MVCRB; // Scale to maximum signal: useful for motor emgNotchLT = NFLT.step(emgLT.read() ); // Notch filter emgHPLT = HPFLT.step(emgNotchLT); // High-pass filter: also normalises around 0. emgAbsHPLT = abs(emgHPLT); // Take absolute value emgLPLT = LPFLT.step(emgAbsHPLT); // Low-pass filter: creates envelope emgMEANSUBLT = emgLPLT - RESTMEANLT; //substract the restmean value LTF = emgLPLT/MVCLT; // Scale to maximum signal: useful for motor emgNotchRT = NFRT.step(emgRT.read() ); // Notch filter emgHPRT = HPFRT.step(emgNotchRT); // High-pass filter: also normalises around 0. emgAbsHPRT = abs(emgHPRT); // Take absolute value emgLPRT = LPFRT.step(emgAbsHPRT); // Low-pass filter: creates envelope emgMEANSUBRT = emgLPRT - RESTMEANRT; //substract the restmean value RTF = emgLPRT/MVCRT; // Scale to maximum signal: useful for motor //if (emgFiltered >1) //{ // emgFiltered=1.00; //} //pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT); //int maxwaarde = 4096; // = 64x64 //double refP = emgFiltered*maxwaarde; //return refP; // value between 0 and 4096 } void CalibrationEMG() { pc.printf("Timescalibration = %i \r\n",Timescalibration); Timescalibration++; if(Timescalibration<2000) { pc.printf("calibratie rust EMG \r\n"); led = 1; // wait(0.2); //led = 0; emgNotchLB = NFLB.step(emgLB.read() ); emgHPLB = HPFLB.step(emgNotchLB); emgAbsHPLB = abs(emgHPLB); emgLPLB = LPFLB.step(emgAbsHPLB); emgSUMLB += emgLPLB; //SUM all rest values LB emgNotchRB = NFRB.step(emgRB.read()); emgHPRB = HPFRB.step(emgNotchRB); emgAbsHPRB = abs(emgHPRB); emgLPRB = LPFRB.step(emgAbsHPRB); emgSUMRB += emgLPRB; //SUM all rest values RB emgNotchLT = NFLT.step(emgLT.read() ); emgHPLT = HPFLT.step(emgNotchLT); emgAbsHPLT = abs(emgHPLT); emgLPLT = LPFLT.step(emgAbsHPLT); emgSUMLT += emgLPLT; //SUM all rest values LT emgNotchRT = NFRT.step(emgRT.read() ); emgHPRT = HPFRT.step(emgNotchRT); emgAbsHPRT = abs(emgHPRT); emgLPRT = LPFRT.step(emgAbsHPRT); emgSUMRT += emgLPRT; //SUM all rest values RT } if(Timescalibration==1999) { /*led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); */ led = 0; RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value } if(Timescalibration>2000 && Timescalibration<6000) { pc.printf("maximum linker biceps \r\n"); led = 1; /*wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; */ emgNotchLB = NFLB.step(emgLB.read() ); emgHPLB = HPFLB.step(emgNotchLB); emgAbsHPLB = abs(emgHPLB); emgLPLB = LPFLB.step(emgAbsHPLB); double emgfinalLB = emgLPLB; if (emgfinalLB > MVCLB) { //determine what the highest reachable emg signal is MVCLB = emgfinalLB; } } if(Timescalibration>6000 && Timescalibration<10000) { pc.printf(" maximum rechter biceps \r\n"); /*led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); */ led = 0; emgNotchRB = NFRB.step(emgRB.read()); emgHPRB = HPFRB.step(emgNotchRB); emgAbsHPRB = abs(emgHPRB); emgLPRB = LPFRB.step(emgAbsHPRB); double emgfinalRB = emgLPRB; if (emgfinalRB > MVCRB) { //determine what the highest reachable emg signal is MVCRB = emgfinalRB; } } if(Timescalibration>10000 && Timescalibration<14000) { pc.printf("maximum linker triceps \r\n"); led = 1; /*wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; wait(0.2); led = 1; wait(0.2); led = 0; */ emgNotchLT = NFLT.step(emgLT.read() ); emgHPLT = HPFLT.step(emgNotchLT); emgAbsHPLT = abs(emgHPLT); emgLPLT = LPFLT.step(emgAbsHPLT); double emgfinalLT = emgLPLT; if (emgfinalLT > MVCLT) { //determine what the highest reachable emg signal is MVCLT = emgfinalLT; } } if(Timescalibration>14000 && Timescalibration<18000) { pc.printf("maximum rechter triceps"); emgNotchRT = NFRT.step(emgRT.read() ); emgHPRT = HPFRT.step(emgNotchRT); emgAbsHPRT = abs(emgHPRT); emgLPRT = LPFRT.step(emgAbsHPRT); double emgfinalRT = emgLPRT; if (emgfinalRT > MVCRT) { //determine what the highest reachable emg signal is MVCRT = emgfinalRT; } } if(Timescalibration>18000) { pc.printf("calibratie afgelopen"); State = SelectDevice; } // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal); //} //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!! //return maxi; } void RKI() { Rex = cos(q1)*L1 - sin(q2)*L2; Rey = sin(q1)*L1 + cos(q2)*L2; R2x = cos(q1)*L1; R2y = sin(q1)*L1; Fx = (Rsx-Rex)*K; Fy = (Rsy-Rey)*K; Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx; Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx; w1 = Tor1/B1; w2 = Tor2/B2; q1 = q1 + w1*Tijd; q2 = q2 + w2*Tijd; int maxwaarde = 4096; // = 64x64 refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde; refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde; //Get reference positions was eerst 0.5*pi } void SetpointRobot() { double Potmeterwaarde2 = potMeter2.read(); double Potmeterwaarde1 = potMeter1.read(); if (Potmeterwaarde2>0.6) { Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat } else if (Potmeterwaarde2<0.4) { Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat } else { //de x-waarde van de setpoint verandert niet } if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat Rsy += 0.001; } else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 Rsy -= 0.001; } else { //de y-waarde van de setpoint verandert niet } } double GetReferencePosition() { double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden int maxwaarde = 4096; // = 64x64 double refP = Potmeterwaarde*maxwaarde; return refP; // value between 0 and 4096 } double GetReferencePosition2() { double potmeterwaarde2 = potMeter1.read(); int maxwaarde2 = 4096; // = 64x64 double refP2 = potmeterwaarde2*maxwaarde2; return refP2; // value between 0 and 4096 } double FeedBackControl(double error, double &e_prev, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { double kp = 0.0015; // kind of scaled. double Proportional= kp*error; double kd = 0.000008; // kind of scaled. double VelocityError = (error - e_prev)/Ts; double Derivative = kd*VelocityError; e_prev = error; double ki = 0.0001; // kind of scaled. e_int = e_int+Ts*error; double Integrator = ki*e_int; double motorValue = Proportional + Integrator + Derivative; return motorValue; } double FeedBackControl2(double error2, double &e_prev2, double &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { double kp2 = 0.002; // kind of scaled. double Proportional2= kp2*error2; double kd2 = 0.000008; // kind of scaled. double VelocityError2 = (error2 - e_prev2)/Ts; double Derivative2 = kd2*VelocityError2; e_prev2 = error2; double ki2 = 0.00005; // kind of scaled. e_int2 = e_int2+Ts*error2; double Integrator2 = ki2*e_int2; double motorValue2 = Proportional2 + Integrator2 + Derivative2; return motorValue2; } void SetMotor1(double motorValue) { if (motorValue >= 0) { M1D = 0; } else { M1D = 1; } if (fabs(motorValue) > 1) { M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) } else { M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 } } void SetMotor2(double motorValue2) { if (motorValue2 >= 0) { M2D = 1; } else { M2D = 0; } if (fabs(motorValue2) > 1) { M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) } else { M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 } } void MeasureAndControl(void) { SetpointRobot(); // RKI aanroepen RKI(); // hier the control of the 1st control system //double refP = GetReferencePosition(); //KOMT UIT RKI double Huidigepositie = motor1.getPosition(); double error = (refP - Huidigepositie);// make an error double motorValue = FeedBackControl(error, e_prev, e_int); SetMotor1(motorValue); // hier the control of the 2nd control system //double refP2 = GetReferencePosition2(); double Huidigepositie2 = motor2.getPosition(); double error2 = (refP2 - Huidigepositie2);// make an error double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); SetMotor2(motorValue2); pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2); } void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!! { if (RBF>0.3) { goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm? } if (RTF>0.3) { goalx--; } if (LBF>0.3) { goaly++; } if (LTF>0.3) { goaly--; } pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly); } void Loop_funtion() { pc.printf("state machine begint \r\n"); switch(State){ case Cal1: //Calibration motor 1 pc.printf("cal1 \r\n"); State=Cal2; // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1. /* motorValue1 = 0.1f; motorValue2=0; M2E = fabs(motorValue2); M1E = fabs(motorValue1); if (Huidigepositie1== 0) { SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,. //if (fabs(Huidigepositie1<0.01) { State=Cal2; //} } else { SetMotor1(0); Loop_funtion(); }*/ break; case Cal2: //Calibration motor 2 State = CalEMG; /* if (Huidigepositie2== 0) { if (Huidigepositie2<0.01){ State=CalEMG; } else { SetMotor2(0); Loop_funtion(); } */ break; case CalEMG: // Calibration EMG CalibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered. break; case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons if (button==1) { State=EMG; } if (button==0) { State=Demo; } break; case EMG: //Aansturen met EMG pc.printf("EMG begint met aansturen \r\n"); Filteren(); changePosition(); //RKI --> output refP van motor MeasureAndControl(); break; case Demo: // Aansturen met toetsenbord break; } } /*void Tickerfunctie() { //if(caldone == false) //{ pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB); pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, meanrest = %f emgSUMLB %f ,Timescalibration %i\r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB,emgSUMLB, Timescalibration); pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT); pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT); //} } */ int main()//deze moet ooit nog weg --> pas op voor errors { //voor EMG filteren //Left Bicep NFLB.add( &N1LB ); HPFLB.add( &HP1LB ).add( &HP2LB ); LPFLB.add( &LP1LB ).add( &LP2LB ); //Right Bicep NFRB.add( &N1RB ); HPFRB.add( &HP1RB ).add( &HP2RB ); LPFRB.add( &LP1RB ).add( &LP2RB ); //Left Tricep NFLT.add( &N1LT ); HPFLT.add( &HP1LT ).add( &HP2LT ); LPFLT.add( &LP1LT ).add( &LP2LT ); //Right Tricep NFRT.add( &N1RT ); HPFRT.add( &HP1RT ).add( &HP2RT ); LPFRT.add( &LP1RT ).add( &LP2RT ); //voor serial pc.baud(115200); pc.printf("begint met programma \r\n"); //motor M1E.period(PwmPeriod); //set PWMposition at 5000hz //Ticker //Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. // printer.attach(Tickerfunctie,0.4); //State Machine State = Cal1; Position_controller_on = false; Treecko.attach(&Loop_funtion, Ts); while(true) { } //is deze wel nodig? }