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.orig@24:e166f8119dbb, 2018-11-01 (annotated)

Committer:
EvaKrolis
Date:
Thu Nov 01 08:46:57 2018 +0000
Revision:
24:e166f8119dbb
Parent:
20:11fe0aa7f111 
Good version with good Jacobian.; Not been tested yet.

Who changed what in which revision?

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