-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathkinematics
95 lines (70 loc) · 4.48 KB
/
kinematics
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
void Kinematics::triangularInverse(float xTarget,float yTarget, float* aChainLength, float* bChainLength){
/*
The inverse kinematics (relating an xy coordinate pair to the required chain lengths to hit that point)
function for a triangular set up where the chains meet at a point, or are arranged so that they simulate
meeting at a point.
*/
//Confirm that the coordinates are on the wood
_verifyValidTarget(&xTarget, &yTarget);
//Set up variables
float Chain1Angle = 0;
float Chain2Angle = 0;
float Chain1AroundSprocket = 0;
float Chain2AroundSprocket = 0;
float xTangent1 = 0;
float yTangent1 = 0;
float xTangent2 = 0;
float yTangent2 = 0;
//Calculate motor axes length to the bit
float Motor1Distance = sqrt(pow((-1*_xCordOfMotor - xTarget),2)+pow((_yCordOfMotor - yTarget),2));
float Motor2Distance = sqrt(pow((_xCordOfMotor - xTarget),2)+pow((_yCordOfMotor - yTarget),2));
//Calculate the chain angles from horizontal, based on if the chain connects to the sled from the top or bottom of the sprocket
if(sysSettings.chainOverSprocket == 1){
Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) + asin(R/Motor1Distance);
Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) + asin(R/Motor2Distance);
Chain1AroundSprocket = R * Chain1Angle;
Chain2AroundSprocket = R * Chain2Angle;
xTangent1=-1.0*_xCordOfMotor+R*sin(Chain1Angle);
yTangent1=_yCordOfMotor+R*cos(Chain1Angle);
xTangent2=_xCordOfMotor-R*sin(Chain2Angle);
yTangent2=_yCordOfMotor+R*cos(Chain2Angle);
}
else{
Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) - asin(R/Motor1Distance);
Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) - asin(R/Motor2Distance);
Chain1AroundSprocket = R * (3.14159 - Chain1Angle);
Chain2AroundSprocket = R * (3.14159 - Chain2Angle);
xTangent1=-1.0*_xCordOfMotor-R*sin(Chain1Angle);
yTangent1=_yCordOfMotor-R*cos(Chain1Angle);
xTangent2=_xCordOfMotor+R*sin(Chain2Angle);
yTangent2=_yCordOfMotor-R*cos(Chain2Angle);
}
float sledWeight = sysSettings.chainSagCorrection;
float chainWeight = 0.09/304.8;
float chainElasticity = 0.000023;
#TensionDenominator=(x_l y_r- x_r y_l- x_l y_t +x_t y_l +x_r y_t -x_t y_r)
float TensionDenominator= (xTangent1*yTangent2-xTangent2*yTangent1-xTangent1*yTarget+xTarget*yTangent1+xTangent2*yTarget-xTarget*yTangent2);
#T_l = -( w *sqrt( pow(x_l -x_t ,2.0)+pow( y_l -y_t ,2.0)) (x_r -x_t)) /TensionDenominator
float Tension1 = - (sledWeight*sqrt(pow(xTangent1-xTarget,2.0)+pow(yTangent1-yTarget,2.0))*(xTangent2-xTarget))/TensionDenominator;
#T_r = ( w *sqrt( pow(x_r -x_t ,2.0)+pow( y_r -y_t ,2.0)) (x_l -x_t))/(x_ly_r-x_ry_l-x_ly_t+x_ty_l+x_ry_t-x_ty_r)
float Tension2 = (sledWeight*sqrt(pow(xTangent2-xTarget,2.0)+pow(yTangent2-yTarget,2.0))*(xTangent1-xTarget))/TensionDenominator;
float HorizontalTension=Tension1*(xTarget-xTangent1)/Chain1Straight;
#Calculation of horizontal component of tension for two chains should be equal, as validation step
#HorizontalTension1=Tension1*(xTarget-xTangent1)/Chain1Straight;
#HorizontalTension2=Tension2*(xTangent2-xTarget)/Chain2Straight;
#Calculation of vertical force due to tension, to validate tension calculation
#VerticalForce=Tension1*(yTangent1-yTarget)/Chain1Straight+Tension2*(yTangent2-yTarget)/Chain2Straight;
float a1=HorizontalTension/chainWeight;
float a2=HorizontalTension/chainWeight;
#Catenary Equation
float Chain1=sqrt(pow(2*a1*sinh((xTarget-xTangent1)/(2*a1)),2)+pow(yTangent1-yTarget,2));
float Chain2=sqrt(pow(2*a2*sinh((xTangent2-xTarget)/(2*a2)),2)+pow(yTangent2-yTarget,2));
#Calculate total chain lengths accounting for sprocket geometry and chain sag
Chain1 = Chain1AroundSprocket + Chain1*(1.0f+sysSettings.leftChainTolerance/100.0f)/(1+Tension1*chainElasticity);
Chain2 = Chain2AroundSprocket + Chain2*(1.0f+sysSettings.rightChainTolerance/100.0f)/(1+Tension2*chainElasticity);
#Subtract of the virtual length which is added to the chain by the rotation mechanism
Chain1 = Chain1 - sysSettings.rotationDiskRadius;
Chain2 = Chain2 - sysSettings.rotationDiskRadius;
*aChainLength = Chain1;
*bChainLength = Chain2;
}