Eva Krolis
EMG to motor
Dependencies: biquadFilter FastPWM MODSERIAL QEI mbed
main.cpp
- Committer:
- EvaKrolis
- Date:
- 2018-11-02
- Revision:
- 0:aa126aa2ff8b
File content as of revision 0:aa126aa2ff8b:
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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 |
