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:
- 16:c2986e890040
- Parent:
- 15:6566c5dedeeb
- Parent:
- 14:2c0bf576a0e7
- Child:
- 17:b04e1938491a
File content as of revision 16:c2986e890040:
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// Libraries
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.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
// DigitalOut ledBio1(,1); // led 1 Biorobotics shield // LED pins
// DigitalOut ledBio2(,1); // led 2 Biorobotics shield
Ticker blinkTimer; // LED ticker
// Buttons/inputs
InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
InterruptIn buttonK64F(SW3); // button on K64F
InterruptIn buttonEmergency(SW2); // emergency button on K64F
// Motor pins
// PC communication
MODSERIAL pc(USBTX, USBRX); // communication with pc
// Define & initialise state machine
enum states { calibratingMotors, calibratingEMG,
homing, operating, reading, failing, demoing
};
states currentState = calibratingMotors;// start in waiting mode
bool changeState = true; // initialise the first state
//------------------------ 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 ---------------------------
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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
}
// ============================ MOTOR FUNCTIONS ==============================
// ============================= 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 ==============================
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void)
{
switch (currentState) { // determine which state Odin is in // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten
// ========================= 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 #1: when all motor errors smaller than 0.01,
// start calibrating EMG
// if (errorMotorL < 0.01 && errorMotorR < 0.01
// && errorMotorF < 0.01) {
// // 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 1 (x-direction)...\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 #1: after 20 sec in state
if (1) { // CONDITION
// 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 -------------------------- // Evt. ook checken op snelheid als mensen zo dom zijn om tijdens homing op random knopjes te drukken
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 ---------------------------- // Volgorde veranderen voor logica?
// Transition condition #1: with button press, enter demo mode
if (buttonBio1 == true) { // Het is niet nodig om hier ook nog "&& currentState == homing" toe te voegen, want hij bereikt dit stuk code alleen in homing mode
// Actions when leaving state
/* */
currentState = demoing; // change to state
changeState = true; // next loop, switch states
}
// Transition condition #2: with button press, enter operation mode
if (buttonBio2 == true) {
// Actions when leaving state
/* */
currentState = operating; // 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 #1: with button press, back to homing mode // Is het nodig om vanuit operating mode naar homing mode terug te kunnen gaan? -> ja, voor als je een demo wilt starten
if (buttonBio2 == true) {
// Actions when leaving state
blinkTimer.detach();
currentState = homing; // change to state
changeState = true; // next loop, switch states
}
// Transition condition #2: motor angle error < certain value,
// start reading
if (1) { // CONDITION
// Actions when leaving state
blinkTimer.detach();
currentState = homing; // change to state
changeState = true; // next loop, switch states
}
break; // end case
// ============================== READING MODE =============================== // Beweegt deze modus zowel heen als weer en zo ja, hoe bepaalt hij wat moet gebeuren?
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;
}
// ----------------------------- action ------------------------------
// Actions for each loop iteration
/* */
// --------------------------- transition ----------------------------
// Transition condition #1: with button press, back to homing mode // Hier automatisch terug naar operating mode!
if (1) { // En hij gaat nu terug naar homing mode, maar dan moet je dus elke keer weer kiezen voor demo of operation mode?
// Actions when leaving state // CONDITION
/* */
currentState = homing; // 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 #1: with button press, back to homing mode
if (1) {
// Actions when leaving state
/* */
currentState = homing; // change to state
changeState = true; // next loop, switch states
}
// Transition condition #2: after 3 sec relaxation, start reading
if (1) {
// Actions when leaving state
/* */
currentState = reading; // change to state
changeState = true; // next loop, switch states
}
break; // end case
// =============================== FAILING MODE ================================
case failing:
// ------------------------- initialisation --------------------------
if (changeState) { // when entering the state
pc.printf("[ERROR] entering failure mode\r\n");
// print current state
changeState = false; // stay in this state
// Actions when entering state
ledGreen = 1; // fast blinking red LED
ledBlue = 1;
ledRed = 0;
blinkTimer.attach(&blinkLedRed,0.25);
// pin3 = 0; // all motor forces to zero // Pins nog niet gedefiniëerd
// pin5 = 0;
// pin6 = 0;
exit (0); // abort mission
}
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?
// ================================ EMERGENCY ================================
pc.baud(115200); // communication with terminal // Baud rate
// ==================================== LOOP ===================================
while (true) { // loop forever
stateMachine();
}
}
