Eva Krolis / Mbed 2 deprecated Kinematics

Good Jacobian and code Not been tested

Dependencies:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Ramon Waninge

main.cpp@17:e5ca1f228fc5, 2018-10-31 (annotated)

Committer:
Ramonwaninge
Date:
Wed Oct 31 18:01:43 2018 +0000
Revision:
17:e5ca1f228fc5
Parent:
12:8d3bc1fa2321
Child:
18:95611fc90411
Child:
22:31ec06a15ea5
werkt. de vorige werkt niet; de tickerfrequentie is daar te hoog

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 0:779fe292e912 1 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 2 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 3 #include <cmath>
Ramonwaninge 12:8d3bc1fa2321 4 #include "MODSERIAL.h"
Ramonwaninge 0:779fe292e912 5 #define PI 3.14159265
Ramonwaninge 0:779fe292e912 6
Ramonwaninge 3:de8d3ca44a3e 7 MODSERIAL pc(USBTX, USBRX); // connecting to pc
Ramonwaninge 10:2b965defcde5 8 DigitalIn button1(SW3); // defining testbutton NEEDS TO BE REMOVED
Ramonwaninge 12:8d3bc1fa2321 9 DigitalOut ledr(LED1); // Only for testing
Ramonwaninge 10:2b965defcde5 10 //DigitalOut led2(LED2); // Only for testing
Ramonwaninge 10:2b965defcde5 11 InterruptIn button2(SW2); //Only for testing
Ramonwaninge 8:697aa3c94209 12
Ramonwaninge 10:2b965defcde5 13 // nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
Ramonwaninge 0:779fe292e912 14
Ramonwaninge 10:2b965defcde5 15 //Joe dit zijn de inputsignalen (en tussenvariabelen)
Ramonwaninge 12:8d3bc1fa2321 16 //vorige theta
Ramonwaninge 17:e5ca1f228fc5 17 float theta1 = PI*0.49; // huidige/nieuwe theta
Ramonwaninge 17:e5ca1f228fc5 18 float theta4 = PI*0.49;
Ramonwaninge 12:8d3bc1fa2321 19 bool emg1;
Ramonwaninge 12:8d3bc1fa2321 20 bool emg2;
Ramonwaninge 12:8d3bc1fa2321 21 bool emg3;
Ramonwaninge 17:e5ca1f228fc5 22 float thetaflip = 0;
Ramonwaninge 17:e5ca1f228fc5 23 float omega1;
Ramonwaninge 17:e5ca1f228fc5 24 float omega4;
Ramonwaninge 17:e5ca1f228fc5 25 float prefx;
Ramonwaninge 17:e5ca1f228fc5 26 float prefy;
Ramonwaninge 17:e5ca1f228fc5 27 float deltat = 0.01;
Ramonwaninge 12:8d3bc1fa2321 28 //Joe dit zijn de constantes
Ramonwaninge 17:e5ca1f228fc5 29 float ll = 200.0;
Ramonwaninge 17:e5ca1f228fc5 30 float lu = 170.0;
Ramonwaninge 17:e5ca1f228fc5 31 float lb = 10.0;
Ramonwaninge 17:e5ca1f228fc5 32 float le = 79.0;
Ramonwaninge 17:e5ca1f228fc5 33 float xbase = 340;
Ramonwaninge 2:0a7a3c0c08d3 34
Ramonwaninge 10:2b965defcde5 35
Ramonwaninge 2:0a7a3c0c08d3 36 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 10:2b965defcde5 37 //check void forward voor de berekeningen
Ramonwaninge 2:0a7a3c0c08d3 38 //First define the position equation of x
Ramonwaninge 17:e5ca1f228fc5 39 float xendsum;
Ramonwaninge 17:e5ca1f228fc5 40 float xendsqrt1;
Ramonwaninge 17:e5ca1f228fc5 41 float xendsqrt2;
Ramonwaninge 17:e5ca1f228fc5 42 float xend;
Ramonwaninge 17:e5ca1f228fc5 43 float jacobiana;
Ramonwaninge 17:e5ca1f228fc5 44 float jacobianc;
Ramonwaninge 2:0a7a3c0c08d3 45 //Now define the pos. eq. of y
Ramonwaninge 17:e5ca1f228fc5 46 float yendsum;
Ramonwaninge 17:e5ca1f228fc5 47 float yendsqrt1;
Ramonwaninge 17:e5ca1f228fc5 48 float yendsqrt2;
Ramonwaninge 17:e5ca1f228fc5 49 float yend;
Ramonwaninge 17:e5ca1f228fc5 50 float jacobianb;
Ramonwaninge 17:e5ca1f228fc5 51 float jacobiand;
Ramonwaninge 2:0a7a3c0c08d3 52
Ramonwaninge 2:0a7a3c0c08d3 53
Ramonwaninge 10:2b965defcde5 54 //Hier definieren we de functies en tickers
Ramonwaninge 1:f63be2020475 55 Ticker emgcheck;
Ramonwaninge 7:b59b762c537e 56 Ticker emgcheck2;
Ramonwaninge 12:8d3bc1fa2321 57 Ticker rekenen;
Ramonwaninge 1:f63be2020475 58
Ramonwaninge 0:779fe292e912 59
Ramonwaninge 12:8d3bc1fa2321 60 //dit wordt aangeroepen in de tickerfunctie
Ramonwaninge 17:e5ca1f228fc5 61 void inverse(float prex, float prey){
Ramonwaninge 12:8d3bc1fa2321 62 /*
Ramonwaninge 12:8d3bc1fa2321 63 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
Ramonwaninge 12:8d3bc1fa2321 64 ofwel
Ramonwaninge 12:8d3bc1fa2321 65 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
Ramonwaninge 12:8d3bc1fa2321 66 waar Pref = emg signaal
Ramonwaninge 12:8d3bc1fa2321 67 */ //achtergrondinfo hierboven...
Ramonwaninge 12:8d3bc1fa2321 68 //
Ramonwaninge 12:8d3bc1fa2321 69
Ramonwaninge 12:8d3bc1fa2321 70 theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
Ramonwaninge 12:8d3bc1fa2321 71 theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//" "
Ramonwaninge 12:8d3bc1fa2321 72 //Hier worden xend en yend doorgerekend, die formules kan je overslaan
Ramonwaninge 4:49dfbfcd3577 73 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 74 xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
Ramonwaninge 4:49dfbfcd3577 75 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 76 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 12:8d3bc1fa2321 77 //hieronder rekenen we yendeffector door;
Ramonwaninge 6:59744dfe8ea7 78 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 12:8d3bc1fa2321 79 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))));
Ramonwaninge 6:59744dfe8ea7 80 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 81 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 12:8d3bc1fa2321 82
Ramonwaninge 3:de8d3ca44a3e 83 }
Ramonwaninge 12:8d3bc1fa2321 84 //deze onderstaande tickerfunctie wordt aangeroepen
Ramonwaninge 12:8d3bc1fa2321 85 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 86 {
Ramonwaninge 12:8d3bc1fa2321 87
Ramonwaninge 12:8d3bc1fa2321 88 //Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
Ramonwaninge 12:8d3bc1fa2321 89
Ramonwaninge 12:8d3bc1fa2321 90 jacobiana = (500*(-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 91 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.))/
Ramonwaninge 12:8d3bc1fa2321 92 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 93 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 94 (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.))/
Ramonwaninge 12:8d3bc1fa2321 95 (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)))/
Ramonwaninge 12:8d3bc1fa2321 96 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 97 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 98 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.))/
Ramonwaninge 12:8d3bc1fa2321 99 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 100 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 101 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.))/
Ramonwaninge 12:8d3bc1fa2321 102 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 103 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 104 (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.) -
Ramonwaninge 12:8d3bc1fa2321 105 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.))/
Ramonwaninge 12:8d3bc1fa2321 106 (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. -
Ramonwaninge 12:8d3bc1fa2321 107 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 108 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 109 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 110 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.))/
Ramonwaninge 12:8d3bc1fa2321 111 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 112 (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)))/
Ramonwaninge 12:8d3bc1fa2321 113 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 114
Ramonwaninge 12:8d3bc1fa2321 115 jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 116 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.))/
Ramonwaninge 12:8d3bc1fa2321 117 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 118 (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)))/
Ramonwaninge 12:8d3bc1fa2321 119 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 120 (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.)*
Ramonwaninge 12:8d3bc1fa2321 121 (-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. +
Ramonwaninge 12:8d3bc1fa2321 122 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 123 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.))/
Ramonwaninge 12:8d3bc1fa2321 124 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 125 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 126 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.))/
Ramonwaninge 12:8d3bc1fa2321 127 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 128 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 129 (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.) -
Ramonwaninge 12:8d3bc1fa2321 130 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.))/
Ramonwaninge 12:8d3bc1fa2321 131 (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. -
Ramonwaninge 12:8d3bc1fa2321 132 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 133 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 134 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 135 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.))/
Ramonwaninge 12:8d3bc1fa2321 136 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 137 (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)))/
Ramonwaninge 12:8d3bc1fa2321 138 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 139
Ramonwaninge 12:8d3bc1fa2321 140 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.))/
Ramonwaninge 12:8d3bc1fa2321 141 (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. -
Ramonwaninge 12:8d3bc1fa2321 142 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 143 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
Ramonwaninge 12:8d3bc1fa2321 144 (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.)*
Ramonwaninge 12:8d3bc1fa2321 145 (-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. +
Ramonwaninge 12:8d3bc1fa2321 146 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 147 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.))/
Ramonwaninge 12:8d3bc1fa2321 148 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 149 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 150 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.))/
Ramonwaninge 12:8d3bc1fa2321 151 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 152 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 153 (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.) -
Ramonwaninge 12:8d3bc1fa2321 154 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.))/
Ramonwaninge 12:8d3bc1fa2321 155 (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. -
Ramonwaninge 12:8d3bc1fa2321 156 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 157 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 158 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 159 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.))/
Ramonwaninge 12:8d3bc1fa2321 160 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 161 (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)))/
Ramonwaninge 12:8d3bc1fa2321 162 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 163
Ramonwaninge 12:8d3bc1fa2321 164 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)))/
Ramonwaninge 12:8d3bc1fa2321 165 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 166 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 167 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.))/
Ramonwaninge 12:8d3bc1fa2321 168 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 169 (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.)*
Ramonwaninge 12:8d3bc1fa2321 170 (-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. +
Ramonwaninge 12:8d3bc1fa2321 171 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 172 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.))/
Ramonwaninge 12:8d3bc1fa2321 173 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 174 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 175 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.))/
Ramonwaninge 12:8d3bc1fa2321 176 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 177 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 178 (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.) -
Ramonwaninge 12:8d3bc1fa2321 179 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.))/
Ramonwaninge 12:8d3bc1fa2321 180 (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. -
Ramonwaninge 12:8d3bc1fa2321 181 ((-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.))/
Ramonwaninge 12:8d3bc1fa2321 182 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 183 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 184 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.))/
Ramonwaninge 12:8d3bc1fa2321 185 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 186 (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)))/
Ramonwaninge 12:8d3bc1fa2321 187 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 188
Ramonwaninge 12:8d3bc1fa2321 189 //vanaf hier weer door met lezen!
Ramonwaninge 12:8d3bc1fa2321 190 prefx = 1*(!button1); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
Ramonwaninge 12:8d3bc1fa2321 191 // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
Ramonwaninge 12:8d3bc1fa2321 192 prefy = 1*(!button2); //sw2,
Ramonwaninge 12:8d3bc1fa2321 193 inverse(prefx, prefy);
Ramonwaninge 17:e5ca1f228fc5 194 ledr!=ledr;
Ramonwaninge 12:8d3bc1fa2321 195 }
Ramonwaninge 12:8d3bc1fa2321 196
Ramonwaninge 12:8d3bc1fa2321 197
Ramonwaninge 12:8d3bc1fa2321 198
Ramonwaninge 12:8d3bc1fa2321 199
Ramonwaninge 12:8d3bc1fa2321 200
Ramonwaninge 12:8d3bc1fa2321 201
Ramonwaninge 12:8d3bc1fa2321 202 /*In de nieuwe versie hieronder is forward overbodig geworden, sla maar over
Ramonwaninge 12:8d3bc1fa2321 203
Ramonwaninge 12:8d3bc1fa2321 204 Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 12:8d3bc1fa2321 205 void forward(){ dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
Ramonwaninge 12:8d3bc1fa2321 206 hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
Ramonwaninge 12:8d3bc1fa2321 207 maar daar moet eerst inverse kinematics voor gebeuren.
Ramonwaninge 12:8d3bc1fa2321 208 if (button1 == 0){ als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
Ramonwaninge 12:8d3bc1fa2321 209 theta1 = PI*(theta1/PI + 0.1);
Ramonwaninge 12:8d3bc1fa2321 210 hij is geblokt omdat ik de knop nodig heb
Ramonwaninge 12:8d3bc1fa2321 211 default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
Ramonwaninge 12:8d3bc1fa2321 212 }
Ramonwaninge 12:8d3bc1fa2321 213 else {theta1 = theta1;}
Ramonwaninge 12:8d3bc1fa2321 214 hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding*/
Ramonwaninge 12:8d3bc1fa2321 215
Ramonwaninge 12:8d3bc1fa2321 216 /*void demomode(){} //Komt nog...
Ramonwaninge 12:8d3bc1fa2321 217
Ramonwaninge 12:8d3bc1fa2321 218 als emg2 == voorbij treshold,
Ramonwaninge 17:e5ca1f228fc5 219 float theta1 -> plus counts (emg*richting)
Ramonwaninge 17:e5ca1f228fc5 220 float theta4 -> plus counts (emg*richting)
Ramonwaninge 12:8d3bc1fa2321 221 reken y door
Ramonwaninge 12:8d3bc1fa2321 222
Ramonwaninge 12:8d3bc1fa2321 223 void flip(){
Ramonwaninge 10:2b965defcde5 224 if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
Ramonwaninge 12:8d3bc1fa2321 225 }suppressed omdat ik button 2 nodig heb...*/
Ramonwaninge 12:8d3bc1fa2321 226
Ramonwaninge 7:b59b762c537e 227
Ramonwaninge 10:2b965defcde5 228 // de beweging voor de xcoordinaat!
Ramonwaninge 12:8d3bc1fa2321 229
Ramonwaninge 10:2b965defcde5 230
Ramonwaninge 12:8d3bc1fa2321 231 //tot aan hier overslaan
Ramonwaninge 10:2b965defcde5 232
Ramonwaninge 0:779fe292e912 233
Ramonwaninge 10:2b965defcde5 234
Ramonwaninge 0:779fe292e912 235 int main()
Ramonwaninge 0:779fe292e912 236 {
Ramonwaninge 12:8d3bc1fa2321 237 //Initial conditions
Ramonwaninge 12:8d3bc1fa2321 238
Ramonwaninge 12:8d3bc1fa2321 239 theta1 = PI*0.49;
Ramonwaninge 12:8d3bc1fa2321 240 theta4 = PI*0.49;
Ramonwaninge 2:0a7a3c0c08d3 241 pc.baud(115200);
Ramonwaninge 12:8d3bc1fa2321 242 //default = theta1 = theta4 = pi/2,
Ramonwaninge 17:e5ca1f228fc5 243 emgcheck.attach(kinematics, 0.0085); // roep de ticker aan (
Ramonwaninge 17:e5ca1f228fc5 244
Ramonwaninge 12:8d3bc1fa2321 245 pc.printf("%f", theta1);
Ramonwaninge 12:8d3bc1fa2321 246 while(true) {
Ramonwaninge 12:8d3bc1fa2321 247
Ramonwaninge 12:8d3bc1fa2321 248
Ramonwaninge 12:8d3bc1fa2321 249 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
Ramonwaninge 12:8d3bc1fa2321 250 wait(0.5);//anders krijg je DOEZEND waardes...
Ramonwaninge 12:8d3bc1fa2321 251
Ramonwaninge 12:8d3bc1fa2321 252
Ramonwaninge 8:697aa3c94209 253 }
Ramonwaninge 0:779fe292e912 254 }