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:
- 51:78c75cc72d17
- Parent:
- 50:4a7b0a3f64cb
- Parent:
- 49:0ada5a721686
- Child:
- 52:17b3aeacb643
diff -r 4a7b0a3f64cb -r 78c75cc72d17 main.cpp
--- a/main.cpp Thu Nov 01 20:22:46 2018 +0000
+++ b/main.cpp Thu Nov 01 20:30:19 2018 +0000
@@ -56,8 +56,9 @@
// Define & initialise state machine
const float dt = 0.002f;
enum states { calibratingMotorL, calibratingMotorR, calibratingMotorF,
- calibratingEMG, homing, reading, operating, failing, demoing };
-
+ calibratingEMG, homing, reading, operating, failing, demoing
+ };
+
states currentState = calibratingMotorL; // start in motor L mode
bool changeState = true; // initialise the first state
@@ -156,6 +157,9 @@
int countsCalibratedL;
int countsCalibratedR;
int countsCalibratedF;
+int countsOffsetL;
+int countsOffsetR;
+int countsOffsetF;
float angleCurrentL;
float angleCurrentR;
float angleCurrentF;
@@ -379,19 +383,16 @@
// ------------------------ 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;
}
@@ -405,7 +406,7 @@
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
@@ -438,15 +439,29 @@
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 = (pot2*2.0f*PI)-PI;
-// return angle;
-//}
+float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{
+ float angleL = (pot1*2.0f*PI)-PI;
+ return angleL;
+}
+
+int countsInputCalibratedL() // Gets the counts from encoder 1
+{
+ countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
+ return countsL;
+}
-float countsCalibrCalcL(int countsOffsetL)
+void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
{
- countsCalibratedL = countsL - countsOffsetL + countsCalibration;
- return countsCalibratedL;
+ 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
@@ -456,6 +471,7 @@
{
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
@@ -464,26 +480,14 @@
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);
+ countsOffsetL = countsL;
+ countsL = countsL - countsOffsetL + countsCalibration;
+ //countsL = 0;
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
-}
-
// ------------------------ 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
{
@@ -508,15 +512,29 @@
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 angleDesiredR() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{
+ float angleR = (pot2*2.0f*PI)-PI;
+ return angleR;
+}
+
+int countsInputCalibratedR() // Gets the counts from encoder 1
+{
+ countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
+ return countsR;
+}
-float countsCalibrCalcR(int countsOffsetR)
+void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
{
- countsCalibratedR = countsR - countsOffsetR + countsCalibration;
- return countsCalibratedR;
+ float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
+ angleReferenceR = -angleReferenceR; // different minus sign per motor
+ countsR = countsInputCalibratedR(); // different encoder pins per motor
+ angleCurrentR = angleCurrent(countsR); // different minus sign per motor
+ errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
+
+ float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
+ pin5 = fabs(PIDControlR); // different pins for every motor
+ pin4 = PIDControlR > 0.0f; // different pins for every motor
}
void calibrationR() // Partially the same as motorTurnR, only with potmeter input
@@ -526,378 +544,364 @@
{
float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
angleReferenceR = -angleReferenceR; // different minus sign per motor
+ countsR = countsInputR(); // different encoder pins 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
+ pin5 = fabs(PIDControlR); // different pins for every motor
+ pin4 = 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
+ countsOffsetR = countsR;
+ countsR = countsR - countsOffsetR + countsCalibration;
+ //countsR = 0;
+ pin5 = 0.0f;
+ // BUTTON PRESS: TO NEXT 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) {
- switch (currentState) { // determine which state Odin is in
-
-// ========================= MOTOR CALIBRATION MODE ==========================
- case calibratingMotors:
-// ------------------------- initialisation --------------------------
- if (changeState) { // when entering the state
- pc.printf("[MODE] calibrating motors...\r\n");
- // print current state
- changeState = false; // stay in this state
+void stateMachine(void)
+{
+ switch (currentState) { // determine which state Odin is in
- // Actions when entering state
- ledRed = 1; // cyan-green blinking LED
- ledGreen = 0;
- ledBlue = 0;
- blinkTimer.attach(&blinkLedBlue,0.5);
+// ======================== MOTOR L CALIBRATION MODE =========================
+ case calibratingMotorL:
+// ------------------------- initialisation --------------------------
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] calibrating motor L...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- }
+ // Actions when entering state
+ ledRed = 1; // green blinking LED
+ ledGreen = 1;
+ ledBlue = 1;
+ blinkTimer.attach(&blinkLedGreen,0.5);
+
+ }
// ----------------------------- action ------------------------------
- // Actions for each loop iteration
- /* */
+ // Actions for each loop iteration
+ calibrationL();
// --------------------------- transition ----------------------------
- // Transition condition: when all motor errors smaller than 0.01,
- // start calibrating EMG
- if (errorMotorL < 0.01 && errorMotorR < 0.01
- && errorMotorF < 0.01 && buttonHome == false) {
+ // Transition condition: when all motor errors smaller than 0.01,
+ // start calibrating EMG
+ if (errorL < 0.01F && buttonBio1 == true) {
+
+ // Actions when leaving state
+ blinkTimer.detach();
- // Actions when leaving state
- blinkTimer.detach();
+ currentState = calibratingMotorR; // change to state
+ changeState = true; // next loop, switch states
+ }
+
+ break; // end case
+
+// ======================== MOTOR R CALIBRATION MODE =========================
+ case calibratingMotorR:
+// ------------------------- initialisation --------------------------
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] calibrating motor R...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- currentState = calibratingEMG; // change to state
- changeState = true; // next loop, switch states
- }
+ // Actions when entering state
+ ledRed = 1; // cyan-green blinking LED
+ ledGreen = 0;
+ ledBlue = 1;
+ blinkTimer.attach(&blinkLedBlue,0.5);
- break; // end case
+ }
+// ----------------------------- action ------------------------------
+ // Actions for each loop iteration
+ calibrationR();
+// --------------------------- transition ----------------------------
+ // Transition condition: when all motor errors smaller than 0.01,
+ // start calibrating EMG
+ if (errorR < && buttonBio2 == true) {
-// =========================== EMG CALIBRATION MODE ===========================
- case calibratingEMG:
+ // Actions when leaving state
+ blinkTimer.detach();
+
+ currentState = calibratingMotorF; // change to state
+ changeState = true; // next loop, switch states
+ }
+
+ break; // end case
+
+// ======================== MOTOR F CALIBRATION MODE =========================
+ case calibratingMotorF:
// ------------------------- initialisation --------------------------
- if (changeState) { // when entering the state
- pc.printf("[MODE] calibrating EMG...\r\n");
- // print current state
- changeState = false; // stay in this state
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] calibrating motor F...\r\n");
+ // print current state
+ changeState = false; // stay in this state
+
+ // Actions when entering state
+ ledRed = 1; // green blinking LED
+ ledGreen = 1;
+ ledBlue = 1;
+ blinkTimer.attach(&blinkLedGreen,0.5);
- // Actions when entering state
- ledRed = 1; // cyan-blue blinking LED
- ledGreen = 0;
- ledBlue = 0;
- blinkTimer.attach(&blinkLedGreen,0.5);
+ }
+// ----------------------------- action ------------------------------
+ // Actions for each loop iteration
+ calibrationF();
+// --------------------------- transition ----------------------------
+ // Transition condition: when all motor errors smaller than 0.01,
+ // start calibrating EMG
+ if (errorF < 0.01f && buttonHome == false) {
+
+ // Actions when leaving state
+ blinkTimer.detach();
+
+ currentState = calibratingEMG; // change to state
+ changeState = true; // next loop, switch states
+ }
- FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
- FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
+ break; // end case
+
+// =========================== EMG CALIBRATION MODE ===========================
+ case calibratingEMG:
+// ------------------------- initialisation --------------------------
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] calibrating EMG...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- EMGtransition_timer.reset();
- EMGtransition_timer.start();
- }
+ // Actions when entering state
+ ledRed = 1; // cyan-blue blinking LED
+ ledGreen = 0;
+ ledBlue = 0;
+ blinkTimer.attach(&blinkLedGreen,0.5);
+
+ FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
+ FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
+
+ EMGtransition_timer.reset();
+ EMGtransition_timer.start();
+ }
// ----------------------------- action ------------------------------
- // Actions for each loop iteration
- CalibrateEMG0(); //start emg calibration every 0.005 seconds
- CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
- /* */
+ // Actions for each loop iteration
+ CalibrateEMG0(); //start emg calibration every 0.005 seconds
+ CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
+ /* */
// --------------------------- transition ----------------------------
- // Transition condition: after 20 sec in state
- if (local_timer.read() > 20) {
- // Actions when leaving state
- blinkTimer.detach();
+ // Transition condition: after 20 sec in state
+ if (EMGtransition_timer.read() > 20) {
+ // Actions when leaving state
+ blinkTimer.detach();
- currentState = homing; // change to state
- changeState = true; // next loop, switch states
- }
- break; // end case
+ currentState = homing; // change to state
+ changeState = true; // next loop, switch states
+ }
+ break; // end case
// ============================== HOMING MODE ================================
- case homing:
+ case homing:
// ------------------------- initialisation --------------------------
- if (changeState) { // when entering the state
- pc.printf("[MODE] homing...\r\n");
- // print current state
- changeState = false; // stay in this state
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] homing...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- // Actions when entering state
- ledRed = 1; // cyan LED on
- ledGreen = 0;
- ledBlue = 0;
+ // Actions when entering state
+ ledRed = 1; // cyan LED on
+ ledGreen = 0;
+ ledBlue = 0;
- }
+ }
// ----------------------------- action ------------------------------
- // Actions for each loop iteration
- /* */
+ // Actions for each loop iteration
+ /* */
// --------------------------- transition ----------------------------
- // Transition condition #1: with button press, enter demo mode,
- // but only when velocity == 0
- if (errorMotorL < 0.01 && errorMotorR < 0.01
- && errorMotorF < 0.01 && buttonBio1 == true) {
- // Actions when leaving state
- /* */
+ // Transition condition #1: with button press, enter reading mode,
+ // but only when velocity == 0
+ if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
+ && errorCalibratedF < 0.01f && buttonBio1 == false) {
+ // Actions when leaving state
+ /* */
- currentState = reading; // change to state
- changeState = true; // next loop, switch states
- }
- // Transition condition #2: with button press, enter operation mode
- // but only when velocity == 0
- if (errorMotorL < 0.01 && errorMotorR < 0.01
- && errorMotorF < 0.01 && buttonBio2 == true) {
- // Actions when leaving state
- /* */
+ currentState = reading; // change to state
+ changeState = true; // next loop, switch states
+ }
+ // Transition condition #2: with button press, enter demo mode
+ // but only when velocity == 0
+ if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
+ && errorCalibratedF < 0.01f && buttonBio2 == false) {
+ // Actions when leaving state
+ /* */
- currentState = demoing; // change to state
- changeState = true; // next loop, switch states
- }
- break; // end case
+ currentState = demoing; // change to state
+ changeState = true; // next loop, switch states
+ }
+ break; // end case
// ============================== READING MODE ===============================
- case reading:
+ case reading:
// ------------------------- initialisation --------------------------
- if (changeState) { // when entering the state
- pc.printf("[MODE] reading...\r\n");
- // print current state
- changeState = false; // stay in this state
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] reading...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- // Actions when entering state
- ledRed = 1; // blue LED on
- ledGreen = 1;
- ledBlue = 0;
+ // Actions when entering state
+ ledRed = 1; // blue LED on
+ ledGreen = 1;
+ ledBlue = 0;
- // TERUGKLAPPEN
+ // TERUGKLAPPEN
- }
+ }
// ----------------------------- action ------------------------------
- // Actions for each loop iteration
- ReadUseEMG0(); //Start the use of EMG
- ReadUseEMG1(); //Start the use of EMG
- /* */
+ // Actions for each loop iteration
+ ReadUseEMG0(); //Start the use of EMG
+ ReadUseEMG1(); //Start the use of EMG
+ /* */
// --------------------------- transition ----------------------------
- // Transition condition #1: when EMG signal detected, enter reading
- // mode
- if (xMove == true || yMove == true) {
- // Actions when leaving state
- /* */
+ // Transition condition #1: when EMG signal detected, enter operating
+ // mode
+ if (xMove == true && yMove == true) {
+ // Actions when leaving state
+ /* */
- currentState = reading; // change to state
- changeState = true; // next loop, switch states
- }
- // Transition condition #2: with button press, back to homing mode
- if (buttonHome == false) {
- // Actions when leaving state
- /* */
+ currentState = operating; // change to state
+ changeState = true; // next loop, switch states
+ }
+ // Transition condition #2: with button press, back to homing mode
+ if (buttonHome == false) {
+ // Actions when leaving state
+ /* */
- currentState = homing; // change to state
- changeState = true; // next loop, switch states
- }
- break; // end case
+ currentState = homing; // change to state
+ changeState = true; // next loop, switch states
+ }
+ break; // end case
// ============================= OPERATING MODE ==============================
- case operating:
+ case operating:
// ------------------------- initialisation --------------------------
- if (changeState) { // when entering the state
- pc.printf("[MODE] operating...\r\n");
- // print current state
- changeState = false; // stay in this state
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] operating...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- // Actions when entering state
- ledRed = 1; // blue fast blinking LED
- ledGreen = 1;
- ledBlue = 1;
- blinkTimer.attach(&blinkLedBlue,0.25);
+ // Actions when entering state
+ ledRed = 1; // blue fast blinking LED
+ ledGreen = 1;
+ ledBlue = 1;
+ blinkTimer.attach(&blinkLedBlue,0.25);
- }
+ }
// ----------------------------- action ------------------------------
- // Actions for each loop iteration
- /* */
+ // Actions for each loop iteration
+ /* */
// --------------------------- transition ----------------------------
- // Transition condition: when path is over, back to reading mode
- if (errorMotorL < 0.01 && errorMotorR < 0.01) {
- // Actions when leaving state
- blinkTimer.detach();
+ // Transition condition: when path is over, back to reading mode
+ if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f) {
+ // Actions when leaving state
+ blinkTimer.detach();
- currentState = reading; // change to state
- changeState = true; // next loop, switch states
- }
- break; // end case
+ currentState = reading; // change to state
+ changeState = true; // next loop, switch states
+ }
+ break; // end case
// ============================== DEMOING MODE ===============================
- case demoing:
+ case demoing:
// ------------------------- initialisation --------------------------
- if (changeState) { // when entering the state
- pc.printf("[MODE] demoing...\r\n");
- // print current state
- changeState = false; // stay in this state
+ if (changeState) { // when entering the state
+ pc.printf("[MODE] demoing...\r\n");
+ // print current state
+ changeState = false; // stay in this state
- // Actions when entering state
- ledRed = 0; // yellow LED on
- ledGreen = 0;
- ledBlue = 1;
+ // Actions when entering state
+ ledRed = 0; // yellow LED on
+ ledGreen = 0;
+ ledBlue = 1;
- }
+ }
// ----------------------------- action ------------------------------
- // Actions for each loop iteration
- /* */
- ReadUseEMG0(); //Start the use of EMG
- ReadUseEMG1(); //Start the use of EMG
+ // Actions for each loop iteration
+ /* */
+ ReadUseEMG0(); //Start the use of EMG
+ ReadUseEMG1(); //Start the use of EMG
+ kinematics();
+ motorTurnL();
+ motorTurnR();
// --------------------------- transition ----------------------------
- // Transition condition: with button press, back to homing mode
- if (buttonHome == false) {
- // Actions when leaving state
- /* */
+ // Transition condition: with button press, back to homing mode
+ if (buttonHome == false) {
+ // Actions when leaving state
+ /* */
- currentState = homing; // change to state
- changeState = true; // next loop, switch states
- }
- break; // end case
+ currentState = homing; // change to state
+ changeState = true; // next loop, switch states
+ }
+ break; // end case
// =============================== FAILING MODE ================================
- case failing:
- changeState = false; // stay in this state
+ case failing:
+ changeState = false; // stay in this state
- // Actions when entering state
- ledRed = 0; // red LED on
- ledGreen = 1;
- ledBlue = 1;
+ // Actions when entering state
+ ledRed = 0; // red LED on
+ ledGreen = 1;
+ ledBlue = 1;
- pin3 = 0; // all motor forces to zero
- pin5 = 0;
- pin6 = 0;
- exit (0); // stop all current functions
- break; // end case
+ pin3 = 0; // all motor forces to zero
+ pin5 = 0;
+ pin6 = 0;
+ exit (0); // stop all current functions
+ break; // end case
// ============================== DEFAULT MODE =================================
- default:
+ default:
// ---------------------------- enter failing mode -----------------------------
- currentState = failing; // change to state
- changeState = true; // next loop, switch states
- // print current state
- pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
+ currentState = failing; // change to state
+ changeState = true; // next loop, switch states
+ // print current state
+ pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
- } // end switch
- } // end stateMachine
+ } // end switch
+} // end stateMachine
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
- int main() {
+int main()
+{
// ================================ EMERGENCY ================================
- //If the emergency button is pressed, stop program via failing state
- buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
+ //If the emergency button is pressed, stop program via failing state
+ buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
// ============================= PC-COMMUNICATION ============================
- pc.baud(115200); // communication with terminal
- pc.printf("\n\n[START] starting O.D.I.N\r\n");
+ pc.baud(115200); // communication with terminal
+ pc.printf("\n\n[START] starting O.D.I.N\r\n");
// ============================= PIN DEFINE PERIOD ===========================
- // If you give a period on one pin, c++ gives all pins this period
- pin3.period_us(15);
+ // If you give a period on one pin, c++ gives all pins this period
+ pin3.period_us(15);
// ==================================== LOOP ==================================
- // run state machine at 500 Hz
- stateTimer.attach(&stateMachine,dt);
+ // run state machine at 500 Hz
+ stateTimer.attach(&stateMachine,dt);
// =============================== ADD FILTERS ===============================
- //Make filter chain for the first EMG
- filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
- //Make filter chain for the second EMG
- filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
- }
+ //Make filter chain for the first EMG
+ filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
+ //Make filter chain for the second EMG
+ filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
+}
