forked from naubiomech/ExoCode
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Reference_ADJ.h
50 lines (39 loc) · 1.14 KB
/
Reference_ADJ.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
// In this file we adjust he torque reference as a function of the steps as introduced in the A_EXO_s_2_0_2
#ifndef REFERENCE_ADJ_HEADER
#define REFERENCE_ADJ_HEADER
void ref_step_adj(Leg* leg) {
if (leg->activate_in_3_steps == 1) {
if (leg->first_step == 1) {
leg->coef_in_3_steps = 0;
leg->first_step = 0;
}
if ((leg->state == 3) && (leg->state_old == 1) && (leg->start_step == 0)) {
leg->start_step = 1;
leg->start_time = millis();
}
if (leg->start_step == 1) {
if ((leg->state == 1) && (leg->state_old == 3)) {
leg->start_step = 0;
if (millis() - leg->start_time >= step_time_length) { // if the transition from 3 to 1 lasted more than 0.3 sec it was a step
leg->num_3_steps += 1;
}
}
}
leg->coef_in_3_steps = leg->num_3_steps / 6;
if (leg->coef_in_3_steps >= 1) {
leg->coef_in_3_steps = 1;
leg->activate_in_3_steps = 0;
leg->first_step = 1;
leg->num_3_steps = 0;
leg->start_step = 0;
}
}
return;
}
void R_ref_step_adj() {
ref_step_adj(right_leg);
}
void L_ref_step_adj() {
ref_step_adj(left_leg);
}
#endif