-
Notifications
You must be signed in to change notification settings - Fork 0
/
exo_mech.h
137 lines (113 loc) · 2.68 KB
/
exo_mech.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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
#pragma once
#include "exo_glob.h"
/* IMPORTANT:
Suppose that to two equal motors have:
- equal spine lengths;
- equal positive rotation side;
*/
// Quadrilateral constants (mm)
#if SERVO == 1
#define QUADRL_A 31.0
#elif SERVO == 2
#define QUADRL_A 38.0
#endif
#if(SIDE)
#define QUADRL_C 68.0
#define QUADRL_D 33.0
#else
#define QUADRL_C 63.0
#define QUADRL_D 35.0
#endif
// Deformation of B due to gamma
#if(SIDE)
#if SERVO == 1
#define Z0_H_TO_B 2.55293212e+01
#define Z1_H_TO_B 1.68968056e+00
#define Z2_H_TO_B -4.02988833e-03
#elif SERVO == 2
#endif
#else
#if SERVO == 1
#define Z0_H_TO_B 1.26535467e+02
#define Z1_H_TO_B 5.96779280e-01
#define Z2_H_TO_B -1.16798202e-03
#elif SERVO == 2
#define Z0_H_TO_B 9.18135289e+01
#define Z1_H_TO_B 9.05979936e-01
#define Z2_H_TO_B -1.93742482e-03
#endif
#endif
// Angle offsets (deg)
#if(SIDE)
#if SERVO == 1
#define TETA_TO_90_POS_SERVO 106.0 // value of teta when servo position is 90 deg
#elif SERVO == 2
#endif
#else
#if SERVO == 1
#define TETA_TO_90_POS_SERVO 101.0
#elif SERVO == 2
#define TETA_TO_90_POS_SERVO 102.0
#endif
#endif
// Rotation direction
#if(SIDE)
#if SERVO == 1
#define T_INC_POS_INC 1
#elif SERVO == 2
#define T_INC_POS_INC 0
#endif
#else
#if SERVO == 1
#define T_INC_POS_INC 0
#elif SERVO == 2
#define T_INC_POS_INC 1
#endif
#endif
// Conversion servo position <-> teta
#if T_INC_POS_INC == 1
#define TETA_TO_POS_SERVO(T) (90. + ((T) - TETA_TO_90_POS_SERVO))
#define POS_SERVO_TO_TETA(P) (-90. + (P) + TETA_TO_90_POS_SERVO)
#elif T_INC_POS_INC == 0
#define TETA_TO_POS_SERVO(T) (90. - ((T) - TETA_TO_90_POS_SERVO))
#define POS_SERVO_TO_TETA(P) (90. - (P) + TETA_TO_90_POS_SERVO)
#endif
// Servo limits (deg)
#if(SIDE)
#if SERVO == 1
#define SERVO_MIN_POS 10
#define SERVO_MAX_POS 140
#elif SERVO == 2
#endif
#else
#if SERVO == 1
#define SERVO_MIN_POS 20
#define SERVO_MAX_POS 130
#elif SERVO == 2
#define SERVO_MIN_POS 30
#define SERVO_MAX_POS 150
#endif
#endif
#define SERVO_MIN_BLOCK_POS (SERVO_MIN_POS+10)
#define SERVO_MAX_BLOCK_POS (SERVO_MAX_POS-10)
// Ankle angle limits (deg)
#define AAN_MIN_POS -25
#define AAN_MAX_POS 25
#if T_INC_POS_INC == 1
#define AAN_MIN_BLOCK_POS SERVO_MAX_BLOCK_POS
#define AAN_MAX_BLOCK_POS SERVO_MIN_BLOCK_POS
#elif T_INC_POS_INC == 0
#define AAN_MIN_BLOCK_POS SERVO_MIN_BLOCK_POS
#define AAN_MAX_BLOCK_POS SERVO_MAX_BLOCK_POS
#endif
// Trigonometric constants
#define RAD_TO_DEG 57.29577951308232 // deg/rad
#define DEG_TO_RAD 0.017453292519943295 // rad/deg
#ifdef __cplusplus
extern "C" {
#endif
float calcGAMMA(float H, float B);
float calcB_H(float H);
#ifdef __cplusplus
}
#endif