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@2:0a7a3c0c08d3, 2018-10-29 (annotated)

Committer:
Ramonwaninge
Date:
Mon Oct 29 14:02:47 2018 +0000
Revision:
2:0a7a3c0c08d3
Parent:
1:f63be2020475
Child:
3:de8d3ca44a3e
De formule werkt; xcoordinaat wordt gedisplayed

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 2:0a7a3c0c08d3 4 #include "MODSERIAL.h"
Ramonwaninge 0:779fe292e912 5 #define PI 3.14159265
Ramonwaninge 0:779fe292e912 6
Ramonwaninge 2:0a7a3c0c08d3 7 MODSERIAL pc(USBTX, USBRX);
Ramonwaninge 2:0a7a3c0c08d3 8 DigitalIn button1(SW3);
Ramonwaninge 2:0a7a3c0c08d3 9 DigitalIn button2(SW2);
Ramonwaninge 2:0a7a3c0c08d3 10 DigitalOut LED(LED1);
Ramonwaninge 0:779fe292e912 11
Ramonwaninge 0:779fe292e912 12 //Joe dit zijn de inputsignalen
Ramonwaninge 0:779fe292e912 13 double theta1; double theta4; double emg1; double emg2; double emg3;
Ramonwaninge 0:779fe292e912 14 // Joe dit zijn de outputsignalen
Ramonwaninge 1:f63be2020475 15 double x; double y;
Ramonwaninge 1:f63be2020475 16
Ramonwaninge 0:779fe292e912 17 //Joe dit zijn de constantes
Ramonwaninge 2:0a7a3c0c08d3 18 double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 340;
Ramonwaninge 0:779fe292e912 19 //Joe dit zijn de Tickers
Ramonwaninge 2:0a7a3c0c08d3 20
Ramonwaninge 2:0a7a3c0c08d3 21 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 2:0a7a3c0c08d3 22 //First define the position equation of x
Ramonwaninge 2:0a7a3c0c08d3 23 double xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 2:0a7a3c0c08d3 24 double 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 2:0a7a3c0c08d3 25 double xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 26 double xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 2:0a7a3c0c08d3 27 //Now define the pos. eq. of y
Ramonwaninge 2:0a7a3c0c08d3 28 double yendsum = le;
Ramonwaninge 2:0a7a3c0c08d3 29 double 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 2:0a7a3c0c08d3 30 double yendsqrt2 = ll/5*(sin(theta1)+sin(theta4))+ sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 2:0a7a3c0c08d3 31 float yend = (xendsum + xendsqrt1/xendsqrt2);
Ramonwaninge 2:0a7a3c0c08d3 32
Ramonwaninge 2:0a7a3c0c08d3 33
Ramonwaninge 2:0a7a3c0c08d3 34 //Hier definieren we de functies
Ramonwaninge 1:f63be2020475 35 Ticker emgcheck;
Ramonwaninge 1:f63be2020475 36
Ramonwaninge 0:779fe292e912 37
Ramonwaninge 0:779fe292e912 38 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 0:779fe292e912 39 void xcor(){
Ramonwaninge 2:0a7a3c0c08d3 40 //als emg1==voorbij treshold,
Ramonwaninge 2:0a7a3c0c08d3 41 theta1 = PI*0.33; //double theta1-> plus een paar counts (emg*richting)
Ramonwaninge 2:0a7a3c0c08d3 42 theta4 = PI*0.5; //double theta4-> plus een paar counts (emg*richting)
Ramonwaninge 2:0a7a3c0c08d3 43 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 1:f63be2020475 44 //default = als x = xbase/2... break
Ramonwaninge 0:779fe292e912 45 //end
Ramonwaninge 0:779fe292e912 46 }
Ramonwaninge 1:f63be2020475 47 void ycor(){
Ramonwaninge 1:f63be2020475 48 //als emg2 == voorbij treshold,
Ramonwaninge 1:f63be2020475 49 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 50 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 51 //reken y door
Ramonwaninge 1:f63be2020475 52 //default = als y = default... break
Ramonwaninge 1:f63be2020475 53 //end
Ramonwaninge 1:f63be2020475 54 }
Ramonwaninge 0:779fe292e912 55 void flip(){}
Ramonwaninge 0:779fe292e912 56
Ramonwaninge 0:779fe292e912 57 int main()
Ramonwaninge 0:779fe292e912 58 {
Ramonwaninge 2:0a7a3c0c08d3 59
Ramonwaninge 2:0a7a3c0c08d3 60 pc.baud(115200);
Ramonwaninge 2:0a7a3c0c08d3 61 pc.printf("%f\n", xend);
Ramonwaninge 0:779fe292e912 62 //default = theta1 = theta4 = pi/2
Ramonwaninge 0:779fe292e912 63 emgcheck.attach(xcor, 0.001);
Ramonwaninge 0:779fe292e912 64 emgcheck.attach(ycor, 0.001);
Ramonwaninge 0:779fe292e912 65 emgcheck.attach(flip, 0.001);
Ramonwaninge 0:779fe292e912 66
Ramonwaninge 0:779fe292e912 67
Ramonwaninge 0:779fe292e912 68
Ramonwaninge 2:0a7a3c0c08d3 69
Ramonwaninge 0:779fe292e912 70 }