Tommie Verouden
/
StateMachine
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: biquadFilter FastPWM MODSERIAL QEI mbed
main.cpp
- Committer:
- tverouden
- Date:
- 2018-11-01
- Revision:
- 50:4a7b0a3f64cb
- Parent:
- 44:ca74d11a2dac
- Child:
- 51:78c75cc72d17
File content as of revision 50:4a7b0a3f64cb:
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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 pot1(A1);
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 -------------------------
//Constants
const double ll = 230; //Length of the lower arm
const double lu = 198; //Length of the upper arm
const double lb = 50; //Length of the part between the upper arms
const double le = 79; //Length of the end-effector beam
const double xbase = 450-lb; //Length between the motors
//General parameters
double theta1 = PI*0.49; //Angle of the left motor
double theta4 = PI*0.49; //Angle of the right motor
double thetaflip = 0; //Angle of the flipping motor
double prefx; //Desired x velocity
double prefy; //Desired y velocity "
float iJ[2][2]; //inverse Jacobian matrix
//Kinematics parameters for x
float xendsum;
float xendsqrt1;
float xendsqrt2;
float xend;
//Kinematics parameters for y
float yendsum;
float yendsqrt1;
float yendsqrt2;
float yend;
// ---------------------- Parameters for the motors ---------------------------
const float countsRad = 4200.0f;
int countsL;
int countsR;
int countsF;
int countsCalibratedL;
int countsCalibratedR;
int countsCalibratedF;
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
}
// ============================= 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
}
// ========================= KINEMATICS FUNCTIONS ============================
//forward kinematics function , &xend and¥d are output.
void kinematicsForward(float &xend_, float ¥d_, float theta1_, float theta4_)
{
//Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output
float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
float yendsqrt1_ = (-xbase/ll + cos(theta1_)+cos(theta4_))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1_)+cos(theta4_))- ll*(1+cos(theta1_+theta4_))));
float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
}
//Below we have the inverse kinematics function.
void kinematicsInverse(float prex, float prey)
{
theta1 += (prefx*(iJ[0][0])-iJ[0][1]*prey)*dt; //theta 1 is itself + the desired speeds in x and y direction, both
theta4 += (prefx*iJ[1][0]-iJ[1][1]*prey)*dt; //multiplied with a prefactor which comes out of the motor
//the iJ values are defined in the "kinematics" function
//Calling the forward kinematics, to calculate xend and yend
kinematicsForward(xend,yend,theta1,theta4);
}
void kinematics()
{
float xend1,xend2,xend3,yend1,yend2,yend3;
const float dq = 0.001; //benadering van 'delta' hoek
kinematicsForward(xend1,yend1,theta1,theta4);
kinematicsForward(xend2,yend2,theta1+dq,theta4);
kinematicsForward(xend3,yend3,theta1,theta4+dq);
float a,b,c,d;
a = xend2-xend1;
b = xend3-xend1;
c = yend2-yend1;
d = yend3-yend1;
float Q = 1/(a*d-b*c)/dq;
iJ[0][0] = d*Q;
iJ[0][1]= -c*Q;
iJ[1][0] = -b*Q;
iJ[1][1] = a*Q;
prefx = 0.001*xMove; //sw3, Prefx has multiplier one, // Gain aanpassen eventueel??
// but that has to become a value
// dependant on the motor
prefy = 0.001*yMove; //sw2,
kinematicsInverse(prefx, prefy);
}
// these values are printed for controlling purposes (can later be removed)
void printvalue()
{
pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 = %f\n\r",xend, yend,theta1,theta4);
//pc.printf("%f\n\r",xend1);
}
// ============================ 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
{
int countsL;
countsL = encoderL.getPulses();
return countsL;
}
int countsInputR() // Gets the counts from encoder 2
{
int countsR;
countsR = encoderR.getPulses();
return countsR;
}
int countsInputF() // Gets the counts from encoder 3
{
int countsF;
countsF = encoderF.getPulses();
return countsF;
}
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;
}
float errorCalc(float angleReference,float angleCurrent) // Calculates the error of the system, based on the current angle and the reference value
{
float error = angleReference - angleCurrent;
return error;
}
// ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
{
float Kp = 19.24f;
float Ki = 1.02f;
float Kd = 0.827f;
float error = errorCalc(angleReference,angleCurrent);
static float errorIntegralL = 0.0;
static float errorPreviousL = error; // initialization with this value only done once!
static BiQuad PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
float u_k = Kp * error;
// Integral part
errorIntegralL = errorIntegralL + error * dt;
float u_i = Ki * errorIntegralL;
// Derivative part
float errorDerivative = (error - errorPreviousL)/dt;
float errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
float u_d = Kd * errorDerivativeFiltered;
errorPreviousL = error;
// Sum all parts and return it
return u_k + u_i + u_d;
}
//float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
//{ float angle = (pot2*2.0f*PI)-PI;
// return angle;
//}
float countsCalibrCalcL(int countsOffsetL)
{
countsCalibratedL = countsL - countsOffsetL + countsCalibration;
return countsCalibratedL;
}
void calibrationL() // Partially the same as motorTurnL, only with potmeter input
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
{
float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
angleReferenceL = -angleReferenceL; // different minus sign 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) {
int countsOffsetL = countsL;
countsCalibrCalcL(countsOffsetL);
pin6 = 0.0f;
// BUTTON PRESS: TO NEXT STATE
}
}
void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
{
float angleReferenceL = theta1; // insert kinematics output here instead of angleDesiredL()
angleReferenceL = -angleReferenceL; // different minus sign per motor
int countsL = countsInputL(); // different encoder pins per motor
angleCurrentL = angleCurrent(countsCalibratedL); // different minus sign per motor
errorCalibratedL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
pin6 = fabs(PIDControlL); // different pins for every motor
pin7 = PIDControlL > 0.0f; // different pins for every motor
}
// ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
float PIDControllerR(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
{
float Kp = 19.24f;
float Ki = 1.02f;
float Kd = 0.827f;
float error = errorCalc(angleReference,angleCurrent);
static float errorIntegralR = 0.0;
static float errorPreviousR = error; // initialization with this value only done once!
static BiQuad PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
float u_k = Kp * error;
// Integral part
errorIntegralR = errorIntegralR + error * dt;
float u_i = Ki * errorIntegralR;
// Derivative part
float errorDerivative = (error - errorPreviousR)/dt;
float errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
float u_d = Kd * errorDerivativeFiltered;
errorPreviousR = error;
// Sum all parts and return it
return u_k + u_i + u_d;
}
//float angleDesiredR() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
//{ float angle = (pot2*2.0f*PI)-PI;
// return angle;
//}
float countsCalibrCalcR(int countsOffsetR)
{
countsCalibratedR = countsR - countsOffsetR + countsCalibration;
return countsCalibratedR;
}
void calibrationR() // Partially the same as motorTurnR, only with potmeter input
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
{
float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
angleReferenceR = -angleReferenceR; // different minus sign per motor
angleCurrentR = angleCurrent(countsR); // different minus sign per motor
errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
if (fabs(errorR) >= 0.01f) {
float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
pin6 = fabs(PIDControlR); // different pins for every motor
pin7 = PIDControlR > 0.0f; // different pins for every motor
} else if (fabs(errorR) < 0.01f) {
int countsOffsetR = countsR;
countsCalibrCalcR(countsOffsetR);
pin6 = 0.0f;
// BUTTON PRESS: NAAR VOLGENDE STATE
}
}
void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
{
float angleReferenceR = theta4; // insert kinematics output here instead of angleDesiredR()
angleReferenceR = -angleReferenceR; // different minus sign per motor
int countsR = countsInputR(); // different encoder pins per motor
angleCurrentR = angleCurrent(countsCalibratedR); // different minus sign per motor
errorCalibratedR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
pin6 = fabs(PIDControlR); // different pins for every motor
pin7 = PIDControlR > 0.0f; // different pins for every motor
}
// ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
{
float Kp = 19.24f;
float Ki = 1.02f;
float Kd = 0.827f;
float error = errorCalc(angleReference,angleCurrent);
static float errorIntegralF = 0.0;
static float errorPreviousF = error; // initialization with this value only done once!
static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
float u_k = Kp * error;
// Integral part
errorIntegralF = errorIntegralF + error * dt;
float u_i = Ki * errorIntegralF;
// Derivative part
float errorDerivative = (error - errorPreviousF)/dt;
float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
float u_d = Kd * errorDerivativeFiltered;
errorPreviousF = error;
// Sum all parts and return it
return u_k + u_i + u_d;
}
float angleDesiredF() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
{
float angle = (pot1*2.0f*PI)-PI;
return angle;
}
float countsCalibrCalcF(int countsOffsetF)
{
countsCalibratedF = countsF - countsOffsetF + countsCalibration;
return countsCalibratedF;
}
void calibrationF() // Partially the same as motorTurnF, only with potmeter input
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
{
float angleReferenceF = 0.0f;
//float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
angleReferenceF = -angleReferenceF; // different minus sign per motor
angleCurrentF = angleCurrent(countsF); // different minus sign per motor
errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
if (fabs(errorF) >= 0.01f) {
float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
pin6 = fabs(PIDControlF); // different pins for every motor
pin7 = PIDControlF > 0.0f; // different pins for every motor
} else if (fabs(errorF) < 0.01f) {
int countsOffsetF = countsF;
countsCalibrCalcF(countsOffsetF);
pin6 = 0.0f;
// BUTTON PRESS: TO NEXT STATE
}
}
void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
{
float angleReferenceF = 0.0f;
//float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
angleReferenceF = -angleReferenceF; // different minus sign per motor
int countsF = countsInputF(); // different encoder pins per motor
angleCurrentF = angleCurrent(countsCalibratedF); // different minus sign per motor
errorCalibratedF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
pin6 = fabs(PIDControlF); // different pins for every motor
pin7 = PIDControlF > 0.0f; // different pins for every motor
}
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void) {
switch (currentState) { // determine which state Odin is in
// ========================= MOTOR CALIBRATION MODE ==========================
case calibratingMotors:
// ------------------------- initialisation --------------------------
if (changeState) { // when entering the state
pc.printf("[MODE] calibrating motors...\r\n");
// print current state
changeState = false; // stay in this state
// Actions when entering state
ledRed = 1; // cyan-green blinking LED
ledGreen = 0;
ledBlue = 0;
blinkTimer.attach(&blinkLedBlue,0.5);
}
// ----------------------------- action ------------------------------
// Actions for each loop iteration
/* */
// --------------------------- transition ----------------------------
// Transition condition: when all motor errors smaller than 0.01,
// start calibrating EMG
if (errorMotorL < 0.01 && errorMotorR < 0.01
&& errorMotorF < 0.01 && buttonHome == false) {
// 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 (local_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 demo mode,
// but only when velocity == 0
if (errorMotorL < 0.01 && errorMotorR < 0.01
&& errorMotorF < 0.01 && buttonBio1 == true) {
// Actions when leaving state
/* */
currentState = reading; // change to state
changeState = true; // next loop, switch states
}
// Transition condition #2: with button press, enter operation mode
// but only when velocity == 0
if (errorMotorL < 0.01 && errorMotorR < 0.01
&& errorMotorF < 0.01 && buttonBio2 == true) {
// Actions when leaving state
/* */
currentState = 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 reading
// mode
if (xMove == true || yMove == true) {
// Actions when leaving state
/* */
currentState = reading; // change to state
changeState = true; // next loop, switch states
}
// Transition condition #2: with button press, back to homing mode
if (buttonHome == false) {
// Actions when leaving state
/* */
currentState = 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 (errorMotorL < 0.01 && errorMotorR < 0.01) {
// 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
// --------------------------- 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);
}
