Eva Krolis
EMG to motor
Dependencies: biquadFilter FastPWM MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 0:aa126aa2ff8b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,882 @@
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+// Libraries
+#include "mbed.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include <algorithm>
+#include <math.h>
+#include <cmath>
+#define PI 3.14159265
+
+// LEDs
+DigitalOut ledRed(LED_RED,1); // red LED K64F
+DigitalOut ledGreen(LED_GREEN,1); // green LED K64F
+DigitalOut ledBlue(LED_BLUE,1); // blue LED K64F
+
+Ticker blinkTimer; // LED ticker
+Timer EMGtransition_timer; // Timer for the transition from EMG calibration to homing
+
+// Buttons/inputs
+InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
+InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
+InterruptIn buttonEmergency(SW2); // emergency button on K64F
+InterruptIn buttonHome(SW3); // button on K64F
+
+// Potmeters
+AnalogIn pot2(A2);
+
+// Motor pins input
+DigitalIn pin8(D8); // Encoder L B
+DigitalIn pin9(D9); // Encoder L A
+DigitalIn pin10(D10); // Encoder R B
+DigitalIn pin11(D11); // Encoder R A
+DigitalIn pin12(D12); // Encoder F B
+DigitalIn pin13(D13); // Encoder F A
+
+// Motor pins output
+DigitalOut pin2(D2); // Motor F direction = motor flip
+FastPWM pin3(A5); // Motor F pwm = motor flip
+DigitalOut pin4(D4); // Motor R direction = motor right
+FastPWM pin5(D5); // Motor R pwm = motor right
+FastPWM pin6(D6); // Motor L pwm = motor left
+DigitalOut pin7(D7); // Motor L direction = motor left
+
+// Utilisation of libraries
+MODSERIAL pc(USBTX, USBRX); // communication with pc
+QEI encoderL(D9,D8,NC,4200); // Encoder input of motor L
+QEI encoderR(D10,D11,NC,4200); // Encoder input of motor R
+QEI encoderF(D12,D13,NC,4200); // Encoder input of motor F
+Ticker motorL;
+Ticker motorR;
+Ticker motorF;
+
+// Define & initialise state machine
+const float dt = 0.002f;
+enum states { calibratingMotorL, calibratingMotorR, calibratingMotorF,
+ calibratingEMG, homing, reading, operating, failing, demoing
+ };
+
+states currentState = calibratingMotorL; // start in motor L mode
+bool changeState = true; // initialise the first state
+
+Ticker stateTimer; // state machine ticker
+
+//------------------------ Parameters for the EMG ----------------------------
+
+// EMG inputs
+AnalogIn EMG0In(A0); // EMG input 0
+AnalogIn EMG1In(A1); // EMG input 1
+
+// Timers
+Ticker FindMax0_timer; // Timer for finding the max muscle
+Ticker FindMax1_timer; // Timer for finding the max muscle
+
+// Constants
+const int Length = 10000; // Length of the array for the calibration
+const int Parts = 50; // Mean average filter over 50 values
+
+// Parameters for the first EMG signal
+float EMG0; // float for EMG input
+float EMG0filt; // float for filtered EMG
+float EMG0filtArray[Parts]; // Array for the filtered array
+float EMG0Average; // float for the value after Moving Average Filter
+float Sum0 = 0; // Sum0 for the moving average filter
+float EMG0Calibrate[Length]; // Array for the calibration
+int ReadCal0 = 0; // Integer to read over the calibration array
+float MaxValue0 = 0; // float to save the max muscle
+float Threshold0 = 0; // Threshold for the first EMG signal
+
+// Parameters for the second EMG signal
+float EMG1; // float for EMG input
+float EMG1filt; // float for filtered EMG
+float EMG1filtArray[Parts]; // Array for the filtered array
+float EMG1Average; // float for the value after Moving Average Filter
+float Sum1 = 0; // Sum0 for the moving average filter
+float EMG1Calibrate[Length]; // Array for the calibration
+int ReadCal1 = 0; // Integer to read over the calibration array
+float MaxValue1 = 0; // float to save the max muscle
+float Threshold1 = 0; // Threshold for the second EMG signal
+
+//Filter variables
+BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
+BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
+BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
+BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
+BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
+BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
+BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
+BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
+BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
+BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
+BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
+BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
+BiQuadChain filter0; //Make chain of filters for the first EMG signal
+BiQuadChain filter1; //Make chain of filters for the second EMG signal
+
+//Bool for movement
+bool xMove = false; //Bool for the x-movement
+bool yMove = false; //Bool for the y-movement
+
+// -------------------- Parameters for the kinematics -------------------------
+double theta1 = PI*0.40; //Angle of the left motor
+double theta4 = PI*0.40; //Angle of the right motor
+// ---------------------- Parameters for the motors ---------------------------
+const float countsRad = 4200.0f;
+int countsL;
+int countsR;
+int countsF;
+int countsCalibratedL;
+int countsCalibratedR;
+int countsCalibratedF;
+int countsOffsetL;
+int countsOffsetR;
+int countsOffsetF;
+float angleCurrentL;
+float angleCurrentR;
+float angleCurrentF;
+float errorL;
+float errorR;
+float errorF;
+float errorCalibratedL;
+float errorCalibratedR;
+float errorCalibratedF;
+
+int countsCalibration = 4200/4;
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+// ============================ GENERAL FUNCTIONS =============================
+void stopProgram(void)
+{
+ // Error message
+ pc.printf("[ERROR] emergency button pressed\r\n");
+ currentState = failing; // change to state
+ changeState = true; // next loop, switch states
+}
+
+void blinkLedRed(void)
+{
+ ledRed =! ledRed; // toggle LED
+}
+void blinkLedGreen(void)
+{
+ ledGreen =! ledGreen; // toggle LED
+}
+void blinkLedBlue(void)
+{
+ ledBlue =! ledBlue; // toggle LED
+}
+
+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;
+}
+
+// ============================= EMG FUNCTIONS ===============================
+
+//Function to read and filter the EMG
+void ReadUseEMG0()
+{
+ for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
+ EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
+ }
+
+ Sum0 = 0;
+ EMG0 = EMG0In; //Save EMG input in float
+ EMG0filt = filter0.step(EMG0); //Filter the signal
+ EMG0filt = abs(EMG0filt); //Take the absolute value
+ EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
+
+ for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
+ Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
+ }
+ EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
+
+ if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
+ xMove = true; //Set movement to true
+ } else {
+ xMove = false; //Otherwise set movement to false
+ }
+}
+
+//Function to read and filter the EMG
+void ReadUseEMG1()
+{
+ for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
+ EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
+ }
+
+ Sum1 = 0;
+ EMG1 = EMG1In; //Save EMG input in float
+ EMG1filt = filter1.step(EMG1); //Filter the signal
+ EMG1filt = abs(EMG1filt); //Take the absolute value
+ EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
+
+ for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
+ Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
+ }
+ EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
+
+ if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
+ yMove = true; //Set y movement to true
+ } else {
+ yMove = false; //Otherwise set y movement to false
+ }
+}
+
+//Function to make an array during the calibration
+void CalibrateEMG0()
+{
+ for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
+ EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
+ }
+
+ Sum0 = 0;
+ EMG0 = EMG0In; //Save EMG input in float
+ EMG0filt = filter0.step(EMG0); //Filter the signal
+ EMG0filt = abs(EMG0filt); //Take the absolute value
+ EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
+
+ for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
+ Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
+ }
+ EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
+
+ ReadCal0++;
+}
+
+//Function to make an array during the calibration
+void CalibrateEMG1()
+{
+ for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
+ EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
+ }
+
+ Sum1 = 0;
+ EMG1 = EMG1In; //Save EMG input in float
+ EMG1filt = filter1.step(EMG1); //Filter the signal
+ EMG1filt = abs(EMG1filt); //Take the absolute value
+ EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
+
+ for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
+ Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
+ }
+ EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
+
+ ReadCal1++;
+}
+
+//Function to find the max value during the calibration
+void FindMax0()
+{
+ MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
+ Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
+ pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
+ FindMax0_timer.detach(); //Detach the timer, so you only use this once
+}
+
+//Function to find the max value during the calibration
+void FindMax1()
+{
+ MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
+ Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
+ pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
+ FindMax1_timer.detach(); //Detach the timer, so you only use this once
+}
+
+// ================ FUNCTIONS TO LINK THE EMG TO THE MOTORS===================
+void EMGMotorLink(){
+ if (xMove == true){
+ theta1 = theta1*0.0001;
+ theta4 = -theta4*0.0001;
+ }
+ if(yMove == true){
+ theta1 = -theta1*0.0001;
+ theta4 = theta4*0.0001;
+ }
+}
+
+// ============================ MOTOR FUNCTIONS ===============================
+// angleDesired now defined as angle of potmeter and not the angle as output from the kinematics
+// 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
+{
+ 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 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 angleDesired() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{
+ float angle = (pot2*2.0f*PI)-PI;
+ return angle;
+}
+
+// ------------------------- 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;
+}
+
+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 = theta1; // insert kinematics output here instead of angleDesired()
+ 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 = angleDesired(); // insert kinematics output here instead of angleDesired()
+ 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
+ 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: TO NEXT 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;
+}
+
+int countsInputCalibratedR() // Gets the counts from encoder 1
+{
+ countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
+ return countsR;
+}
+
+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 angleDesired()
+ 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
+// 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 = angleDesired(); // insert kinematics output here instead of angleDesired()
+ 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
+ pin5 = fabs(PIDControlR); // different pins for every motor
+ pin4 = PIDControlR > 0.0f; // different pins for every motor
+ } else if (fabs(errorR) < 0.01f) {
+ countsOffsetR = countsR;
+ countsR = countsR - countsOffsetR + countsCalibration;
+ //countsR = 0;
+ pin5 = 0.0f;
+ // BUTTON PRESS: TO NEXT STATE
+ }
+}
+
+// ------------------------- 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;
+ }
+
+ int countsInputCalibratedF() // Gets the counts from encoder 1
+ { countsF = encoderF.getPulses()- countsOffsetF + countsCalibration;
+ return countsF;
+ }
+
+void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
+{
+ float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
+ angleReferenceF = -angleReferenceF; // different minus sign per motor
+ countsF = countsInputCalibratedF(); // different encoder pins per motor
+ angleCurrentF = angleCurrent(countsF); // different minus sign per motor
+ errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
+
+ float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
+ pin5 = fabs(PIDControlF); // different pins for every motor
+ pin4 = PIDControlF > 0.0f; // different pins for every motor
+}
+
+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; // insert kinematics output here instead of angleDesired()
+ angleReferenceF = -angleReferenceF; // different minus sign per motor
+ countsF = countsInputF(); // different encoder pins 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
+ pin5 = fabs(PIDControlF); // different pins for every motor
+ pin4 = PIDControlF > 0.0f; // different pins for every motor
+ }
+ else if (fabs(errorF) < 0.01f)
+ { countsOffsetF = countsF;
+ countsF = countsF - countsOffsetF + countsCalibration;
+ //countsF = 0;
+ pin5 = 0.0f;
+ // BUTTON PRESS: TO NEXT STATE
+ }
+}
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+void stateMachine(void)
+{
+ switch (currentState) { // determine which state Odin is in
+
+// ======================== 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
+ calibrationL();
+
+// --------------------------- transition ----------------------------
+ // Transition condition: when all motor errors smaller than 0.01,
+ // start calibrating EMG
+ if (errorL < 0.01f && buttonBio1 == false) {
+
+ // 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
+
+ // Actions when entering state
+ ledRed = 1; // cyan-green blinking LED
+ ledGreen = 0;
+ ledBlue = 1;
+ blinkTimer.attach(&blinkLedBlue,0.5);
+
+ }
+// ----------------------------- action ------------------------------
+ // Actions for each loop iteration
+ calibrationR();
+// --------------------------- transition ----------------------------
+ // Transition condition: when all motor errors smaller than 0.01,
+ // start calibrating EMG
+ if (errorR < 0.01f && buttonBio2 == false) {
+
+ // 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 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);
+
+ }
+// ----------------------------- 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
+ }
+
+ 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
+
+ // 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
+ /* */
+
+
+// --------------------------- transition ----------------------------
+ // 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
+
+// ============================== HOMING MODE ================================
+ case homing:
+// ------------------------- initialisation --------------------------
+ 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;
+
+ }
+// ----------------------------- action ------------------------------
+ // Actions for each loop iteration
+ /* */
+
+// --------------------------- transition ----------------------------
+ // 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 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
+
+// ============================== READING MODE ===============================
+ case reading:
+// ------------------------- initialisation --------------------------
+ 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;
+
+ // TERUGKLAPPEN
+
+ }
+// ----------------------------- action ------------------------------
+ // 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 operating
+ // mode
+ if (xMove == true && yMove == true) {
+ // 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
+
+// ============================= OPERATING MODE ==============================
+ case operating:
+// ------------------------- initialisation --------------------------
+ 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);
+
+ }
+// ----------------------------- action ------------------------------
+ // Actions for each loop iteration
+ /* */
+
+// --------------------------- transition ----------------------------
+ // 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
+
+// ============================== DEMOING MODE ===============================
+ case demoing:
+// ------------------------- initialisation --------------------------
+ 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;
+
+ }
+// ----------------------------- action ------------------------------
+ // Actions for each loop iteration
+ /* */
+ ReadUseEMG0(); //Start the use of EMG
+ ReadUseEMG1(); //Start the use of EMG
+ pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
+ motorTurnL();
+ motorTurnR();
+
+
+// --------------------------- transition ----------------------------
+ // 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
+
+// =============================== FAILING MODE ================================
+ case failing:
+ changeState = false; // stay in this state
+
+ // 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
+
+// ============================== DEFAULT MODE =================================
+ 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");
+
+ } // end switch
+} // end stateMachine
+
+
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+
+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!
+
+// ============================= PC-COMMUNICATION ============================
+ 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);
+
+// ==================================== LOOP ==================================
+ // 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);
+}
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 02 Nov 2018 |
| Imports: | 1 |
| Forks: | 0 |
| Commits: | 1 |
| Dependents: | 0 |
| Dependencies: | 5 |
| Followers: | 4 |
