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
Diff: main.cpp
- Revision:
- 41:5aecc1a27ce6
- Parent:
- 37:317e14b9d551
- Child:
- 42:bb43f1b67787
--- a/main.cpp Thu Nov 01 14:37:47 2018 +0000
+++ b/main.cpp Thu Nov 01 14:51:25 2018 +0000
@@ -6,6 +6,8 @@
#include "MODSERIAL.h"
#include "QEI.h"
#include <algorithm>
+#include <math.h>
+#include <cmath>
#define PI 3.14159265
// LEDs
@@ -81,7 +83,7 @@
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
+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
@@ -92,7 +94,7 @@
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
+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
@@ -132,27 +134,7 @@
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
+float iJ[2][2]; //inverse Jacobian matrix
// ---------------------- Parameters for the motors ---------------------------
const float countsRad = 4200.0f;
@@ -200,97 +182,100 @@
// ============================= 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
+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
+
+ 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
+
+ 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
}
- 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
+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
+
+ 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
+
+ if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
yMove = true; //Set y movement to true
- }
- else{
+ } 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
+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
+
+ 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
+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
+
+ 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(){
+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
@@ -298,7 +283,8 @@
}
//Function to find the max value during the calibration
-void FindMax1(){
+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
@@ -307,123 +293,71 @@
// ========================= KINEMATICS FUNCTIONS ============================
-// Function to calculate the inverse kinematics
-void inverse(double prex, double prey)
+//forward kinematics function , &xend and¥d are output.
+void kinematicsForward(float &xend_, float ¥d_, float theta1_, float theta4_)
{
- /*
- qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
- ofwel
- thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
- waar Pref = emg signaal
- */ //achtergrondinfo hierboven...
- //
+
+ //Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output
+
+ float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
+ float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
+ float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
+ xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
- 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);
+ float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
+ float yendsqrt1_ = (-xbase/ll + cos(theta1_)+cos(theta4_))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1_)+cos(theta4_))- ll*(1+cos(theta1_+theta4_))));
+ float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
+ yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
+}
+
+//Below we have the inverse kinematics function.
+void kinematicsInverse(float prex, float prey)
+{
+
+ theta1 += (prefx*(iJ[0][0])-iJ[0][1]*prey)*dt; //theta 1 is itself + the desired speeds in x and y direction, both
+ theta4 += (prefx*iJ[1][0]-iJ[1][1]*prey)*dt; //multiplied with a prefactor which comes out of the motor
+ //the iJ values are defined in the "kinematics" function
+
+ //Calling the forward kinematics, to calculate xend and yend
+ kinematicsForward(xend,yend,theta1,theta4);
}
-// Function for the Jacobian
void kinematics()
{
+ float xend1,xend2,xend3,yend1,yend2,yend3;
- 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.));
+ const float dq = 0.001; //benadering van 'delta' hoek
+
+ kinematicsForward(xend1,yend1,theta1,theta4);
+ kinematicsForward(xend2,yend2,theta1+dq,theta4);
+ kinematicsForward(xend3,yend3,theta1,theta4+dq);
- 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.));
+ float a,b,c,d;
+ a = xend2-xend1;
+ b = xend3-xend1;
+ c = yend2-yend1;
+ d = yend3-yend1;
+
+ float Q = 1/(a*d-b*c)/dq;
- 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.));
+ iJ[0][0] = d*Q;
+ iJ[0][1]= -c*Q;
+ iJ[1][0] = -b*Q;
+ iJ[1][1] = a*Q;
- 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);
+ prefx = 0.001*xMove; //sw3, Prefx has multiplier one, // Gain aanpassen eventueel??
+ // but that has to become a value
+ // dependant on the motor
+ prefy = 0.001*yMove; //sw2,
+ kinematicsInverse(prefx, prefy);
+}
+
+// these values are printed for controlling purposes (can later be removed)
+void printvalue()
+{
+ pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 = %f\n\r",xend, yend,theta1,theta4);
+ //pc.printf("%f\n\r",xend1);
}
// ============================ MOTOR FUNCTIONS ===============================
@@ -431,240 +365,251 @@
// 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;
- }
+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 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;
- }
+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 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;
- }
+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()
+{
+ 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
+
+ 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;
+ } else if (fabs(errorL) < 0.01f) {
+ int countsOffsetL = countsL;
countsCalibrCalcL(countsOffsetL);
- pin6 = 0.0f;
- // BUTTON PRESS: TO NEXT STATE
- }
-}
-
+ 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 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;
- }
+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()
+{
+ 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
+
+ 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;
+ } else if (fabs(errorR) < 0.01f) {
+ int countsOffsetR = countsR;
countsCalibrCalcR(countsOffsetR);
- pin6 = 0.0f;
- // BUTTON PRESS: NAAR VOLGENDE STATE
- }
-}
-
+ 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 --------------------
- float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
- { float Kp = 19.24f;
- float Ki = 1.02f;
- float Kd = 0.827f;
- float error = errorCalc(angleReference,angleCurrent);
- static float errorIntegralF = 0.0;
- static float errorPreviousF = error; // initialization with this value only done once!
- static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
- // Proportional part:
- float u_k = Kp * error;
- // Integral part
- errorIntegralF = errorIntegralF + error * dt;
- float u_i = Ki * errorIntegralF;
- // Derivative part
- float errorDerivative = (error - errorPreviousF)/dt;
- float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
- float u_d = Kd * errorDerivativeFiltered;
- errorPreviousF = error;
- // Sum all parts and return it
- return u_k + u_i + u_d;
- }
+float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
+{
+ float Kp = 19.24f;
+ float Ki = 1.02f;
+ float Kd = 0.827f;
+ float error = errorCalc(angleReference,angleCurrent);
+ static float errorIntegralF = 0.0;
+ static float errorPreviousF = error; // initialization with this value only done once!
+ static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ // Proportional part:
+ float u_k = Kp * error;
+ // Integral part
+ errorIntegralF = errorIntegralF + error * dt;
+ float u_i = Ki * errorIntegralF;
+ // Derivative part
+ float errorDerivative = (error - errorPreviousF)/dt;
+ float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
+ float u_d = Kd * errorDerivativeFiltered;
+ errorPreviousF = error;
+ // Sum all parts and return it
+ return u_k + u_i + u_d;
+}
- float angleDesiredF() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
- { float angle = (pot1*2.0f*PI)-PI;
- return angle;
- }
-
- float countsCalibrCalcF(int countsOffsetF)
- {
- countsCalibratedF = countsF - countsOffsetF + countsCalibration;
- return countsCalibratedF;
- }
+float angleDesiredF() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{
+ float angle = (pot1*2.0f*PI)-PI;
+ return angle;
+}
+
+float countsCalibrCalcF(int countsOffsetF)
+{
+ countsCalibratedF = countsF - countsOffsetF + countsCalibration;
+ return countsCalibratedF;
+}
void calibrationF() // Partially the same as motorTurnF, only with potmeter input
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
// Do this for every motor and after calibrated all motors, press a button
-{ float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
+{
+ float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
angleReferenceF = -angleReferenceF; // different minus sign per motor
angleCurrentF = angleCurrent(countsF); // different minus sign per motor
errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
-
- if (fabs(errorF) >= 0.01f)
- { float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
+
+ if (fabs(errorF) >= 0.01f) {
+ float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
pin6 = fabs(PIDControlF); // different pins for every motor
pin7 = PIDControlF > 0.0f; // different pins for every motor
- }
- else if (fabs(errorF) < 0.01f)
- { int countsOffsetF = countsF;
+ } else if (fabs(errorF) < 0.01f) {
+ int countsOffsetF = countsF;
countsCalibrCalcF(countsOffsetF);
- pin6 = 0.0f;
- // BUTTON PRESS: TO NEXT STATE
- }
-}
-
+ pin6 = 0.0f;
+ // BUTTON PRESS: TO NEXT STATE
+ }
+}
+
void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
-{
+{
float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
angleReferenceF = -angleReferenceF; // different minus sign per motor
int countsF = countsInputF(); // different encoder pins per motor
angleCurrentF = angleCurrent(countsCalibratedF); // different minus sign per motor
errorCalibratedF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
-
+
float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
pin6 = fabs(PIDControlF); // different pins for every motor
pin7 = PIDControlF > 0.0f; // different pins for every motor
@@ -673,7 +618,7 @@
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void)
{
- switch (currentState) { // determine which state Odin is in
+ switch (currentState) { // determine which state Odin is in
// ========================= MOTOR CALIBRATION MODE ==========================
case calibratingMotors:
@@ -699,7 +644,7 @@
// start calibrating EMG
if (errorMotorL < 0.01 && errorMotorR < 0.01
&& errorMotorF < 0.01 && buttonHome == false) {
-
+
// Actions when leaving state
blinkTimer.detach();
@@ -722,10 +667,10 @@
ledGreen = 0;
ledBlue = 0;
blinkTimer.attach(&blinkLedGreen,0.5);
-
+
FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
-
+
EMGtransition_timer.reset();
EMGtransition_timer.start();
}
@@ -734,11 +679,11 @@
CalibrateEMG0(); //start emg calibration every 0.005 seconds
CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
/* */
-
+
// --------------------------- transition ----------------------------
// Transition condition: after 20 sec in state
- if (local_timer.read() > 20) {
+ if (local_timer.read() > 20) {
// Actions when leaving state
blinkTimer.detach();
@@ -749,7 +694,7 @@
// ============================== HOMING MODE ================================
case homing:
-// ------------------------- initialisation --------------------------
+// ------------------------- initialisation --------------------------
if (changeState) { // when entering the state
pc.printf("[MODE] homing...\r\n");
// print current state
@@ -766,11 +711,11 @@
// Actions for each loop iteration
/* */
-// --------------------------- transition ----------------------------
+// --------------------------- 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) {
+ && errorMotorF < 0.01 && buttonBio1 == true) {
// Actions when leaving state
/* */
@@ -789,7 +734,7 @@
}
break; // end case
-// ============================== READING MODE ===============================
+// ============================== READING MODE ===============================
case reading:
// ------------------------- initialisation --------------------------
if (changeState) { // when entering the state
@@ -801,8 +746,8 @@
ledRed = 1; // blue LED on
ledGreen = 1;
ledBlue = 0;
-
- // TERUGKLAPPEN
+
+ // TERUGKLAPPEN
}
// ----------------------------- action ------------------------------
@@ -813,24 +758,24 @@
// --------------------------- transition ----------------------------
// Transition condition #1: when EMG signal detected, enter reading
- // mode
- if (xMove == true || yMove == true){
- // Actions when leaving state
+ // mode
+ if (xMove == true || yMove == true) {
+ // Actions when leaving state
/* */
currentState = reading; // change to state
changeState = true; // next loop, switch states
}
- // Transition condition #2: with button press, back to homing mode
- if (buttonHome == false) {
- // Actions when leaving state
+ // 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 --------------------------
@@ -852,7 +797,7 @@
// --------------------------- transition ----------------------------
// Transition condition: when path is over, back to reading mode
- if (errorMotorL < 0.01 && errorMotorR < 0.01) {
+ if (errorMotorL < 0.01 && errorMotorR < 0.01) {
// Actions when leaving state
blinkTimer.detach();
@@ -894,17 +839,17 @@
// =============================== FAILING MODE ================================
case failing:
- changeState = false; // stay in this state
+ changeState = false; // stay in this state
- // Actions when entering state
- ledRed = 0; // red LED on
- ledGreen = 1;
- ledBlue = 1;
+ // Actions when entering state
+ ledRed = 0; // red LED on
+ ledGreen = 1;
+ ledBlue = 1;
- pin3 = 0; // all motor forces to zero
- pin5 = 0;
- pin6 = 0;
- exit (0); // stop all current functions
+ pin3 = 0; // all motor forces to zero
+ pin5 = 0;
+ pin6 = 0;
+ exit (0); // stop all current functions
break; // end case
// ============================== DEFAULT MODE =================================
@@ -934,8 +879,8 @@
// ============================= PIN DEFINE PERIOD ============================
// If you give a period on one pin, c++ gives all pins this period
- pin3.period_us(15);
-
+ pin3.period_us(15);
+
// ==================================== LOOP ===================================
// run state machine at 500 Hz
stateTimer.attach(&stateMachine,dt);
