Tommie Verouden
/
StateMachine
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: biquadFilter FastPWM MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 48:1eb0f9ed6cd9
- Parent:
- 47:7919857587f8
- Child:
- 49:0ada5a721686
--- a/main.cpp Thu Nov 01 16:03:01 2018 +0000
+++ b/main.cpp Thu Nov 01 18:46:33 2018 +0000
@@ -156,6 +156,9 @@
int countsCalibratedL;
int countsCalibratedR;
int countsCalibratedF;
+int countsOffsetL;
+int countsOffsetR;
+int countsOffsetF;
float angleCurrentL;
float angleCurrentR;
float angleCurrentF;
@@ -377,255 +380,109 @@
// So, the angleDesired needs to be defined again and the part in which the potmeter value is calculated needs to be commented
// ------------------------ General motor functions ----------------------------
-int countsInputL() // Gets the counts from encoder 1
-{
- int countsL;
- countsL = encoderL.getPulses();
- return countsL;
-}
-int countsInputR() // Gets the counts from encoder 2
-{
- int countsR;
- countsR = encoderR.getPulses();
- return countsR;
-}
-int countsInputF() // Gets the counts from encoder 3
-{
- int countsF;
- countsF = encoderF.getPulses();
- return countsF;
-}
+ int countsInputL() // Gets the counts from encoder 1
+ { countsL = encoderL.getPulses();
+ return countsL;
+ }
+ int countsInputR() // Gets the counts from encoder 2
+ { countsR = encoderR.getPulses();
+ return countsR;
+ }
+ int countsInputF() // Gets the counts from encoder 3
+ { countsF = encoderF.getPulses();
+ return countsF;
+ }
-float angleCurrent(float counts) // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
-{
- float angle = ((float)counts*2.0f*PI)/countsRad;
- while (angle > 2.0f*PI) {
- angle = angle-2.0f*PI;
- }
- while (angle < -2.0f*PI) {
- angle = angle+2.0f*PI;
+ float angleCurrent(float counts) // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
+ { float angle = ((float)counts*2.0f*PI)/countsRad;
+ while (angle > 2.0f*PI)
+ { angle = angle-2.0f*PI;
+ }
+ while (angle < -2.0f*PI)
+ { angle = angle+2.0f*PI;
+ }
+ return angle;
}
- return angle;
-}
-float errorCalc(float angleReference,float angleCurrent) // Calculates the error of the system, based on the current angle and the reference value
-{
- float error = angleReference - angleCurrent;
- return error;
-}
+ float errorCalc(float angleReference,float angleCurrent) // Calculates the error of the system, based on the current angle and the reference value
+ { float error = angleReference - angleCurrent;
+ return error;
+ }
// ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
-float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
-{
- float Kp = 19.24f;
- float Ki = 1.02f;
- float Kd = 0.827f;
- float error = errorCalc(angleReference,angleCurrent);
- static float errorIntegralL = 0.0;
- static float errorPreviousL = error; // initialization with this value only done once!
- static BiQuad PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
- // Proportional part:
- float u_k = Kp * error;
- // Integral part
- errorIntegralL = errorIntegralL + error * dt;
- float u_i = Ki * errorIntegralL;
- // Derivative part
- float errorDerivative = (error - errorPreviousL)/dt;
- float errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
- float u_d = Kd * errorDerivativeFiltered;
- errorPreviousL = error;
- // Sum all parts and return it
- return u_k + u_i + u_d;
-}
+ float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
+ { float Kp = 19.24f;
+ float Ki = 1.02f;
+ float Kd = 0.827f;
+ float error = errorCalc(angleReference,angleCurrent);
+ static float errorIntegralL = 0.0;
+ static float errorPreviousL = error; // initialization with this value only done once!
+ static BiQuad PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ // Proportional part:
+ float u_k = Kp * error;
+ // Integral part
+ errorIntegralL = errorIntegralL + error * dt;
+ float u_i = Ki * errorIntegralL;
+ // Derivative part
+ float errorDerivative = (error - errorPreviousL)/dt;
+ float errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
+ float u_d = Kd * errorDerivativeFiltered;
+ errorPreviousL = error;
+ // Sum all parts and return it
+ return u_k + u_i + u_d;
+ }
-float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-{ float angle = (pot1*2.0f*PI)-PI;
- return angle;
-}
-
-float countsCalibrCalcL(int countsOffsetL)
-{
- countsCalibratedL = countsL - countsOffsetL + countsCalibration;
- return countsCalibratedL;
+ float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+ { float angle = (pot1*2.0f*PI)-PI;
+ return angle;
+ }
+
+ int countsInputCalibratedL() // Gets the counts from encoder 1
+ { countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
+ return countsL;
+ }
+
+void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
+{
+ float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
+ angleReferenceL = -angleReferenceL; // different minus sign per motor
+ countsL = countsInputCalibratedL(); // different encoder pins per motor
+ angleCurrentL = angleCurrent(countsL); // different minus sign per motor
+ errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
+
+ float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
+ pin6 = fabs(PIDControlL); // different pins for every motor
+ pin7 = PIDControlL > 0.0f; // different pins for every motor
}
void calibrationL() // Partially the same as motorTurnL, only with potmeter input
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
-{
- float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
+{ float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
angleReferenceL = -angleReferenceL; // different minus sign per motor
+ countsL = countsInputL(); // different encoder pins per motor
angleCurrentL = angleCurrent(countsL); // different minus sign per motor
errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
-
- if (fabs(errorL) >= 0.01f) {
- float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
+
+ if (fabs(errorL) >= 0.01f)
+ { float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
pin6 = fabs(PIDControlL); // different pins for every motor
pin7 = PIDControlL > 0.0f; // different pins for every motor
- } else if (fabs(errorL) < 0.01f) {
- int countsOffsetL = countsL;
- countsCalibrCalcL(countsOffsetL);
- pin6 = 0.0f;
- // BUTTON PRESS: TO NEXT STATE
}
-}
-
-void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
-{
- float angleReferenceL = theta1; // insert kinematics output here instead of angleDesiredL()
- angleReferenceL = -angleReferenceL; // different minus sign per motor
- int countsL = countsInputL(); // different encoder pins per motor
- angleCurrentL = angleCurrent(countsCalibratedL); // different minus sign per motor
- errorCalibratedL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
-
- float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
- pin6 = fabs(PIDControlL); // different pins for every motor
- pin7 = PIDControlL > 0.0f; // different pins for every motor
+ else if (fabs(errorL) < 0.01f)
+ { countsOffsetL = countsL;
+ countsL = countsL - countsOffsetL + countsCalibration;
+ //countsL = 0;
+ pin6 = 0.0f;
+ // BUTTON PRESS: NAAR VOLGENDE STATE
+ }
}
// ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
-float PIDControllerR(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
-{
- float Kp = 19.24f;
- float Ki = 1.02f;
- float Kd = 0.827f;
- float error = errorCalc(angleReference,angleCurrent);
- static float errorIntegralR = 0.0;
- static float errorPreviousR = error; // initialization with this value only done once!
- static BiQuad PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
- // Proportional part:
- float u_k = Kp * error;
- // Integral part
- errorIntegralR = errorIntegralR + error * dt;
- float u_i = Ki * errorIntegralR;
- // Derivative part
- float errorDerivative = (error - errorPreviousR)/dt;
- float errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
- float u_d = Kd * errorDerivativeFiltered;
- errorPreviousR = error;
- // Sum all parts and return it
- return u_k + u_i + u_d;
-}
-float angleDesiredR() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-{ float angle = (pot2*2.0f*PI)-PI;
- return angle;
-}
-
-float countsCalibrCalcR(int countsOffsetR)
-{
- countsCalibratedR = countsR - countsOffsetR + countsCalibration;
- return countsCalibratedR;
-}
-
-void calibrationR() // Partially the same as motorTurnR, only with potmeter input
-// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
-// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
-// Do this for every motor and after calibrated all motors, press a button
-{
- float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
- angleReferenceR = -angleReferenceR; // different minus sign per motor
- angleCurrentR = angleCurrent(countsR); // different minus sign per motor
- errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
-
- if (fabs(errorR) >= 0.01f) {
- float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
- pin6 = fabs(PIDControlR); // different pins for every motor
- pin7 = PIDControlR > 0.0f; // different pins for every motor
- } else if (fabs(errorR) < 0.01f) {
- int countsOffsetR = countsR;
- countsCalibrCalcR(countsOffsetR);
- pin6 = 0.0f;
- // BUTTON PRESS: NAAR VOLGENDE STATE
- }
-}
-
-void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
-{
- float angleReferenceR = theta4; // insert kinematics output here instead of angleDesiredR()
- angleReferenceR = -angleReferenceR; // different minus sign per motor
- int countsR = countsInputR(); // different encoder pins per motor
- angleCurrentR = angleCurrent(countsCalibratedR); // different minus sign per motor
- errorCalibratedR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
-
- float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
- pin6 = fabs(PIDControlR); // different pins for every motor
- pin7 = PIDControlR > 0.0f; // different pins for every motor
-}
// ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
-float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
-{
- float Kp = 19.24f;
- float Ki = 1.02f;
- float Kd = 0.827f;
- float error = errorCalc(angleReference,angleCurrent);
- static float errorIntegralF = 0.0;
- static float errorPreviousF = error; // initialization with this value only done once!
- static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
- // Proportional part:
- float u_k = Kp * error;
- // Integral part
- errorIntegralF = errorIntegralF + error * dt;
- float u_i = Ki * errorIntegralF;
- // Derivative part
- float errorDerivative = (error - errorPreviousF)/dt;
- float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
- float u_d = Kd * errorDerivativeFiltered;
- errorPreviousF = error;
- // Sum all parts and return it
- return u_k + u_i + u_d;
-}
-
-float angleDesiredF() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-{
- float angle = (pot1*2.0f*PI)-PI;
- return angle;
-}
-
-float countsCalibrCalcF(int countsOffsetF)
-{
- countsCalibratedF = countsF - countsOffsetF + countsCalibration;
- return countsCalibratedF;
-}
-
-void calibrationF() // Partially the same as motorTurnF, only with potmeter input
-// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
-// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
-// Do this for every motor and after calibrated all motors, press a button
- {
- float angleReferenceF = 0.0f;
- //float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
- angleReferenceF = -angleReferenceF; // different minus sign per motor
- angleCurrentF = angleCurrent(countsF); // different minus sign per motor
- errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
-
- if (fabs(errorF) >= 0.01f) {
- float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
- pin6 = fabs(PIDControlF); // different pins for every motor
- pin7 = PIDControlF > 0.0f; // different pins for every motor
- } else if (fabs(errorF) < 0.01f) {
- int countsOffsetF = countsF;
- countsCalibrCalcF(countsOffsetF);
- pin6 = 0.0f;
- // BUTTON PRESS: TO NEXT STATE
- }
- }
-
- void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
- {
- float angleReferenceF = 0.0f;
- //float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
- angleReferenceF = -angleReferenceF; // different minus sign per motor
- int countsF = countsInputF(); // different encoder pins per motor
- angleCurrentF = angleCurrent(countsCalibratedF); // different minus sign per motor
- errorCalibratedF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
-
- float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
- pin6 = fabs(PIDControlF); // different pins for every motor
- pin7 = PIDControlF > 0.0f; // different pins for every motor
- }
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void) {
@@ -650,7 +507,7 @@
// Actions for each loop iteration
/* */
calibrationL();
- calibrationR();
+ //calibrationR();
// --------------------------- transition ----------------------------
// Transition condition: when all motor errors smaller than 0.01,
// start calibrating EMG
@@ -839,7 +696,7 @@
ReadUseEMG1(); //Start the use of EMG
kinematics();
motorTurnL();
- motorTurnR();
+ //motorTurnR();
// --------------------------- transition ----------------------------
// Transition condition: with button press, back to homing mode
