Good Jacobian and code Not been tested
Dependencies: MODSERIAL biquadFilter mbed
Fork of
Kinematics
by 
Ramon Waninge
Diff: main.cpp
- Revision:
- 12:8d3bc1fa2321
- Parent:
- 11:256e84fea925
- Child:
- 13:f77c5f196161
- Child:
- 17:e5ca1f228fc5
--- a/main.cpp Wed Oct 31 09:18:25 2018 +0000
+++ b/main.cpp Wed Oct 31 16:05:53 2018 +0000
@@ -1,37 +1,35 @@
#include "mbed.h"
#include <math.h>
#include <cmath>
-#include "MODSERIAL.h"
+#include "MODSERIAL.h"
#define PI 3.14159265
MODSERIAL pc(USBTX, USBRX); // connecting to pc
DigitalIn button1(SW3); // defining testbutton NEEDS TO BE REMOVED
-DigitalOut ledr(LED1); // Only for testing
+DigitalOut ledr(LED1); // Only for testing
//DigitalOut led2(LED2); // Only for testing
InterruptIn button2(SW2); //Only for testing
// nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
-double theta11;
//Joe dit zijn de inputsignalen (en tussenvariabelen)
-double theta10; //vorige theta
-double theta1; // huidige/nieuwe theta
-double theta40;
-double theta4;
-bool emg1;
-bool emg2;
-bool emg3;
+//vorige theta
+double theta1 = PI*0.49; // huidige/nieuwe theta
+double theta4 = PI*0.49;
+bool emg1;
+bool emg2;
+bool emg3;
double thetaflip = 0;
-double omega1;
-double omega4;
+double omega1;
+double omega4;
double prefx;
double prefy;
double deltat = 0.01;
-//Joe dit zijn de constantes
-double ll = 200.0;
-double lu = 170.0;
-double lb = 10.0;
-double le = 79.0;
+//Joe dit zijn de constantes
+double ll = 200.0;
+double lu = 170.0;
+double lb = 10.0;
+double le = 79.0;
double xbase = 340;
@@ -42,119 +40,216 @@
double xendsqrt1;
double xendsqrt2;
double xend;
+double jacobiana;
+double jacobianc;
//Now define the pos. eq. of y
double yendsum;
double yendsqrt1;
double yendsqrt2;
double yend;
+double jacobianb;
+double jacobiand;
//Hier definieren we de functies en tickers
Ticker emgcheck;
Ticker emgcheck2;
-Ticker emgcheck3;
+Ticker rekenen;
-//Joe, hieronder staan de functies die door de tickers aangeroepen worden
-void forward(){ //dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
- // hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
- //maar daar moet eerst inverse kinematics voor gebeuren.
- /*if (button1 == 0){ //als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
- theta1 = PI*(theta1/PI + 0.1);
- hij is geblokt omdat ik de knop nodig heb
- //default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
- }
- else {theta1 = theta1;}*/
- //hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding
+//dit wordt aangeroepen in de tickerfunctie
+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))));
+ 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);
-
-
-
-
-
- }//end van if xend !=xend
+
}
-void demomode(){} //Komt nog...
- //Alleen nodig in de
- //als emg2 == voorbij treshold,
- //double theta1 -> plus counts (emg*richting)
- //double theta4 -> plus counts (emg*richting)
- //reken y door
- //default = als y = default... break
- //end
-/*void flip(){
+//deze onderstaande tickerfunctie wordt aangeroepen
+void kinematics()
+{
+
+//Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
+
+ jacobiana = (500*(-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
+ ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.))/
+ (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(0.001 + theta1) + sin(theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
+ (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
+ (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
+ ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
+ 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
+ ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
+ ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
+ (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
+
+ jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
+ (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
+ (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
+ (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
+ (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
+ (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
+ ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
+ 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
+ ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
+ ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
+ (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
+
+ jacobianc = (-500*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
+ ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
+ (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
+ (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
+ (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
+ (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
+ ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
+ 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
+ ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
+ ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
+ (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
+
+ jacobiand = (500*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(0.001 + theta1) + sin(theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
+ (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
+ (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
+ (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
+ (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
+ (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
+ ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
+ 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
+ ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
+ (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
+ ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
+ sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
+ (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
+ sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
+
+ //vanaf hier weer door met lezen!
+ prefx = 1*(!button1); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
+ // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
+ prefy = 1*(!button2); //sw2,
+ inverse(prefx, prefy);
+}
+
+
+
+
+
+
+/*In de nieuwe versie hieronder is forward overbodig geworden, sla maar over
+
+Joe, hieronder staan de functies die door de tickers aangeroepen worden
+void forward(){ dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
+ hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
+ maar daar moet eerst inverse kinematics voor gebeuren.
+ if (button1 == 0){ als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
+ theta1 = PI*(theta1/PI + 0.1);
+ hij is geblokt omdat ik de knop nodig heb
+ default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
+ }
+ else {theta1 = theta1;}
+ hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding*/
+
+/*void demomode(){} //Komt nog...
+
+ als emg2 == voorbij treshold,
+ double theta1 -> plus counts (emg*richting)
+ double theta4 -> plus counts (emg*richting)
+ reken y door
+
+void flip(){
if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
-}*/ //suppressed omdat ik button 2 nodig heb...
+}suppressed omdat ik button 2 nodig heb...*/
+
// de beweging voor de xcoordinaat!
-void inverse(){
- /*
- qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
- ofwel
- thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
- waar Pref = emg signaal
- */
- emg1 = false;
- if(theta10 == 0){ //als de hoek voor de beweging niet bekend is
- theta10 = PI*0.5; //Definieer de hoek als half pi !!DIT KAN OOK ERGENS IN DEFAULT STAAN.
- theta1 = theta10;
- }
- if(button2 == 0){emg1 = true;
-
- if(emg1 == true){
- prefx = 1; //derivative of Preference = 1 (needs to be adjusted)
- theta1 = theta10 +/*inverse jacobian keer*/prefx*deltat; //theta nieuw = oud +jacob*deltax
- theta10 = theta1;
- }
- }//sluit van if emg1=true
-
-
-
-}//sluit van void inverse
+
-//dit is voor de ycoordinaat
-void inverse2(){
- emg2 = false; //emg is het inputsignaal van de biceps
- if(theta40 == 0){
- theta40 = PI*0.5;
- theta4 = theta40;
- }
- if(button1 == 0){
- emg2 = true;
- if(emg2 == true){
- prefy = 1;
- theta4 = theta40 + /*inverse jacobian keer*/prefy*deltat; //theta nieuw = oud +jacob*deltax
- theta40 = theta4;
- } //sluit van if emg2==true
- }
+//tot aan hier overslaan
-}
int main()
{
-
+ //Initial conditions
+
+ theta1 = PI*0.49;
+ theta4 = PI*0.49;
pc.baud(115200);
- //default = theta1 = theta4 = pi/2
- emgcheck3.attach(forward, 0.1);
- emgcheck2.attach(inverse2, 0.1);
- emgcheck.attach(inverse, 0.1);
- ledr=1;
- pc.printf("%f", theta10);
- while(true){
-
-
- pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);
- wait(0.5);
-
-
+ //default = theta1 = theta4 = pi/2,
+ emgcheck.attach(kinematics, 0.01); // roep de ticker aan (
+
+
+
+ pc.printf("%f", theta1);
+ while(true) {
+
+
+ pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend); // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
+ wait(0.5);//anders krijg je DOEZEND waardes...
+
+
}
}
\ No newline at end of file
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 01 Nov 2018 |
| Imports: | 1 |
| Forks: | 1 |
| Commits: | 25 |
| Dependents: | 0 |
| Dependencies: | 3 |
| Followers: | 2 |
