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