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:
- efvanmarrewijk
- Date:
- 2018-11-01
- Revision:
- 26:247be0bea9b1
- Parent:
- 25:ac139331fe51
- Child:
- 33:44ba3c159f54
File content as of revision 26:247be0bea9b1:
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// Libraries
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include <algorithm>
#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
// 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 // ESTHER!
// 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 { calibratingMotors, calibratingEMG,
homing, reading, operating, failing, demoing
};
states currentState = calibratingMotors; // start in waiting 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 ReadUseEMG0_timer; // Timer to read, filter and use the EMG
Ticker EMGCalibration0_timer; // Timer for the calibration of the EMG
Ticker FindMax0_timer; // Timer for finding the max muscle
Ticker ReadUseEMG1_timer; // Timer to read, filter and use the EMG
Ticker EMGCalibration1_timer; // Timer for the calibration of the EMG
Ticker FindMax1_timer; // Timer for finding the max muscle
Ticker SwitchState_timer; // Timer to switch from the Calibration to the working mode
// 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.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774); // Make Notch filter around 50 Hz
BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-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 "
double deltat = 0.01; //Time step (dependent on sample frequency)
//Kinematics parameters for x
double xendsum;
double xendsqrt1;
double xendsqrt2;
double xend;
double jacobiana;
double jacobianc;
//Kinematics parameters for y
double yendsum;
double yendsqrt1;
double yendsqrt2;
double yend;
double jacobianb;
double jacobiand;
//Tickers
Ticker kin; //Timer for calculating x,y,theta1,theta4
Ticker simulateval; //Timer that prints the values for x,y, and angles
// ---------------------- 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 ============================
// Function to calculate the inverse kinematics
void inverse(double prex, double prey)
{
/*
qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
ofwel
thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
waar Pref = emg signaal
*/ //achtergrondinfo hierboven...
//
theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//" "
//Hier worden xend en yend doorgerekend, die formules kan je overslaan
xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
xend = (xendsum + xendsqrt1/xendsqrt2)/2;
//hieronder rekenen we yendeffector door;
yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
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))));
yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
yend = (yendsum + yendsqrt1/yendsqrt2);
}
// Function for the Jacobian
void kinematics()
{
jacobiana = (500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.))/
(-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
jacobianb = (-250*(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))))/
(-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
jacobianc = (-500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.))/
(-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
jacobiand = (250*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))))/
(-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
(sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
(sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
(ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
prefx = 1*xMove; //If the EMG is true, the x will move with 1
prefy = 1*yMove; //If the EMG is true, the y will move with 1
inverse(prefx, prefy);
}
// ============================ 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 = (pot1*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 = angleDesiredL(); // 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 = (pot1*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 = angleDesiredR(); // 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 --------------------
// Not yet implemented
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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);
}
// ----------------------------- action ------------------------------
// Actions for each loop iteration
/* */
// --------------------------- transition ----------------------------
// Transition condition: after 20 sec in state
if (1) { // CONDITION -> Eva, hoe moet ik de 20 seconden regelen?
// 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
/* */
// --------------------------- transition ----------------------------
// Transition condition #1: when EMG signal detected, enter reading
// mode
if (moving_average(EMG1 > threshold_value || // Namen variabelen?
moving_average(EMG2 > threshold_value) {
// 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
/* */
// --------------------------- 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 // Pins nog niet gedefiniëerd
// 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);
}
