-
Notifications
You must be signed in to change notification settings - Fork 1
/
Kinematics.h
111 lines (90 loc) · 4.29 KB
/
Kinematics.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
/*This file is part of the Maslow Control Software.
The Maslow Control Software is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Maslow Control Software is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with the Maslow Control Software. If not, see <http://www.gnu.org/licenses/>.
Copyright 2014-2017 Bar Smith*/
#ifndef Kinematics_h
#define Kinematics_h
#define ORIGINCHAINLEN 1650
#include "Arduino.h"
#include "FormatDouble.h"
class Kinematics{
public:
Kinematics();
void inverse (float xTarget,float yTarget, float* aChainLength, float* bChainLength);
void recomputeGeometry();
void forward(float chainALength, float chainBLength, float* xPos, float* yPos);
//geometry
float l = 310.0; //horizontal distance between sled attach points
float s = 139.0; //vertical distance between sled attach points and bit
float h = sqrt((l/2)*(l/2) + s * s); //distance between sled attach point and bit
float h3 = 79.0; //distance from bit to sled center of mass
float D = 2978.4; //distance between sprocket centers
float R = 10.2; //sprocket radius
//machine dimensions
float machineHeight = 1219.2; //this is 4 feet in mm
float machineWidth = 2438.4; //this is 8 feet in mm
float motorOffsetY = 463.0; //vertical distance from the corner of the work area to the sprocket center
float halfWidth = machineWidth / 2.0; //Half the machine width
float halfHeight = machineHeight / 2.0; //Half the machine height
private:
float _moment(float Y1Plus, float Y2Plus, float Phi, float MSinPhi, float MSinPsi1, float MCosPsi1, float MSinPsi2, float MCosPsi2);
float _YOffsetEqn(float YPlus, float Denominator, float Psi);
void _MatSolv();
void _MyTrig();
void _verifyValidTarget(float* xTarget,float* yTarget);
//target router bit coordinates.
float x = 2708.4;
float y = 270;
//utility variables
float DegPerRad = 360/(4 * atan(1));
unsigned long Time;
boolean Mirror;
//Calculation tolerances
float MaxError = 0.001;
byte MaxTries = 10;
float DeltaPhi = 0.001;
float DeltaY = 0.01;
//Criterion Computation Variables
float Phi = -0.2;
float TanGamma;
float TanLambda;
float Y1Plus ;
float Y2Plus;
float Theta = atan(2*s/l);
float Psi1 = Theta - Phi;
float Psi2 = Theta + Phi;
byte Tries = 0;
float Jac[9];
float Solution[3];
float Crit[3];
float Offsetx1;
float Offsetx2;
float Offsety1;
float Offsety2;
float SinPsi1;
float CosPsi1;
float SinPsi2;
float CosPsi2;
float SinPsi1D;
float CosPsi1D;
float SinPsi2D;
float CosPsi2D;
float MySinPhi;
float MySinPhiDelta;
//intermediate output
float Lambda;
float Gamma;
// output = chain lengths measured from 12 o'clock
float Chain1; //left chain length
float Chain2; //right chain length
int i;
};
#endif