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@18:95611fc90411, 2018-10-31 (annotated)

Committer:
Ramonwaninge
Date:
Wed Oct 31 18:36:35 2018 +0000
Revision:
18:95611fc90411
Parent:
17:e5ca1f228fc5
Child:
19:c9f4e4c857e2
Practisch hetzelfde, maar led werkt nu wel 0.005 seconden is te weinig

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