-
Notifications
You must be signed in to change notification settings - Fork 0
/
exo_algos.h
147 lines (123 loc) · 3.92 KB
/
exo_algos.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
138
139
140
141
142
143
144
145
146
147
/**********************
-- NOMENCLATURE:
Outputs:
- MTP: motor target position (deg, see MP)
- AANT: target ankle angle (deg, see AAN)
- GP: gait phase:
- 0: undefined
- 1: 1st rocker
- 2: 2nd rocker
- 3: 3rd rocker
- 4: swing (dorsiflexing)
- 5 swing (dorsiflexed)
- MDET: detach motor (boolean)
Parameters:
- ALGMOD: algorithm mode (0: idle; 1: gait)
- FVANTHS: threshold on FVAN to detect heel-strike
- VACTHS: threshold on VAC to detect heel-strike
- FVANTTO: threshold on FVAN to detect toe-off
- VACTTO: threshold on VAC to detect toe-off
- HOMOD: HO detection mode (1: FVAN/AAN-based; 2: SVAN-based)
- FVANTHO: threshold on FVAN to detect heel-off
- SVANTHO: threshold on SVAN to detect heel-off
- TWNOHO: time window after HS to ignore HO detection (ms)
- MAD: max ankle dorsiflexion angle at end of 2nd rocker (deg, see AAN)
- MAP: max ankle plantarflexion at end of 3rd rocker (deg, see AAN)
- MASW: min ankle during swing (deg, see AAN)
- ST: step time (ms, from heel-strike)
- TOP: Toe-off (% ST)
- TO: Toe-off (ms, from heel-strike)
- E1RP: End of 1st rocker (% ST)
- E1R: end of 1st rocker (ms, from heel-strike)
- E2RP: End of 2nd rocker (% ST)
- E2R: end of 2nd rocker (ms, from heel-strike)
- E3RP: End of 3rd rocker (% ST)
- E3R: end of 3rd rocker (ms, from heel-strike)
- E1SWP: end of 1st part of swing phase with fast dorsiflexion (% ST)
- E1SW: end of 1st part of swing phase with fast dorsiflexion (ms, from heel-strike)
- AANS: starting ankle angle (deg, at heel-strike, see AAN)
- AAN1RP: ankle angle at the start of 1st rocker (deg, at heel-strike, see AAN)
- MTPE: maximum allowed arror between motor target position and current position (deg, see MP)
- AANE: maximum allowed arror between motor target position and current position (deg, see AAN)
**********************/
#pragma once
#include "exo_common.h"
#include <string.h>
#include <stdbool.h>
#include <math.h>
#include "exo_glob.h"
#include "exo_mech.h"
#include "exo_meas.h"
#include "exo_events.h"
#include "exo_pid.h"
#if GAMMA_TO_TETA_DELTA == 1
#include "gamma_to_teta_delta.h"
#elif GAMMA_TO_TETA_DELTA == 2
#include "gamma_to_teta_delta_fast.h"
#elif GAMMA_TO_TETA_DELTA == 3
#include "gamma_to_teta_delta_cse_ofast.h"
#endif
#if SERVO_POS_MEASURE == 2
#include "gamma_to_teta_theor.h"
#endif
/*
#ifdef __cplusplus
extern "C" {
#endif
*/
typedef struct {
float MTP;
float AANT;
unsigned int GP;
bool MDET;
} exo_output;
typedef struct {
unsigned long ALGMOD;
float FVANTHS;
double VACTHS;
float FVANTTO;
double VACTTO;
unsigned long HOMOD;
float FVANTHO;
float SVANTHO;
unsigned long TWNOHO;
float MAD;
float MAP;
float MASW;
unsigned long ST;
float TOP;
unsigned long TO;
float E1RP;
unsigned long E1R;
float E2RP;
unsigned long E2R;
float E3RP;
unsigned long E3R;
float E1SWP;
unsigned long E1SW;
float AANS;
float AANE1R;
float MTPE;
float AANE;
} exo_params;
// ------ ALGORITHMS
void algo1(exo_meas *m, exo_output *o, exo_params *p, exo_params *pv, float (*AANM)(unsigned long, exo_params *, exo_params *, exo_output *));
void algoTrackAANM(exo_meas *m, exo_output *o, exo_params *p, exo_params *pv, float (*AANM)(unsigned long, exo_params *, exo_params *, exo_output *));
// ------
// ------ GENERAL PURPOSE FUNCTIONS
bool detectHS(exo_meas *m, exo_params *p);
bool detectTO(exo_meas *m, exo_params *p);
bool detectHO(exo_meas *m, exo_params *p);
float calcMTP(float AANT, exo_meas *m, exo_params *p);
// ------
// ------ ANKLE ANGLE MODEL
float AANMLinearFixedValues(unsigned long TS, exo_params *p, exo_params *pv, exo_output *o);
float AANMLinearFixedSlopes(unsigned long TS, exo_params *p, exo_params *pv, exo_output *o);
float AANMLinearAdaptive(unsigned long TS, exo_params *p, exo_params *pv, exo_output *o);
float AANMLinearMixed1(unsigned long TS, exo_params *p, exo_params *pv, exo_output *o);
// ------
/*
#ifdef __cplusplus
}
#endif
*/