EMG to motor
Dependencies: biquadFilter FastPWM MODSERIAL QEI mbed
Revision 0:aa126aa2ff8b, committed 2018-11-02
- Comitter:
- EvaKrolis
- Date:
- Fri Nov 02 08:53:55 2018 +0000
- Commit message:
- First try controlling the motors from the EMG without kinematics
Changed in this revision
diff -r 000000000000 -r aa126aa2ff8b BiQuad.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BiQuad.lib Fri Nov 02 08:53:55 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
diff -r 000000000000 -r aa126aa2ff8b FastPWM.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FastPWM.lib Fri Nov 02 08:53:55 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/Sissors/code/FastPWM/#c0b2265cff9c
diff -r 000000000000 -r aa126aa2ff8b MODSERIAL.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Fri Nov 02 08:53:55 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/Sissors/code/MODSERIAL/#da0788f0bd77
diff -r 000000000000 -r aa126aa2ff8b QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Nov 02 08:53:55 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r aa126aa2ff8b main.cpp --- /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); +}
diff -r 000000000000 -r aa126aa2ff8b mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 02 08:53:55 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/675da3299148 \ No newline at end of file