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
Revision 16:c2986e890040, committed 2018-11-01
- Comitter:
- tverouden
- Date:
- Thu Nov 01 10:16:10 2018 +0000
- Parent:
- 15:6566c5dedeeb
- Parent:
- 14:2c0bf576a0e7
- Child:
- 17:b04e1938491a
- Commit message:
- Merged LED & EMG stuff
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp.orig | Show annotated file Show diff for this revision Revisions of this file |
diff -r 6566c5dedeeb -r c2986e890040 main.cpp
--- a/main.cpp Thu Nov 01 10:12:16 2018 +0000
+++ b/main.cpp Thu Nov 01 10:16:10 2018 +0000
@@ -5,6 +5,8 @@
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
+#include <algorithm>
+#define PI 3.14159265
// LEDs
DigitalOut ledRed(LED_RED,1); // red LED K64F
@@ -34,7 +36,97 @@
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 =============================
@@ -61,12 +153,237 @@
// ============================ 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)
diff -r 6566c5dedeeb -r c2986e890040 main.cpp.orig
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig Thu Nov 01 10:16:10 2018 +0000
@@ -0,0 +1,332 @@
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+// Libraries
+#include "mbed.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+
+// 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
+
+
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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 ===============================
+
+
+// ========================= KINEMATICS 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();
+ }
+}
