openDogV3

1
int thresholdStick (int pos) {
2
  
3
    // get zero centre position
4
    pos = pos - 512;  
5

6
    // threshold value for control sticks 
7
    if (pos > 50) {
8
      pos = pos -50;
9
    }
10
    else if (pos < -50) {
11
      pos = pos +50;
12
    }
13
    else {
14
      pos = 0;
15
    }
16

17
    return pos;
18
}
19

20

21

22
// motion filter to filter motions and compliance
23

24
float filter(float prevValue, float currentValue, int filter) {  
25
  float lengthFiltered =  (prevValue + (currentValue * filter)) / (filter + 1);  
26
  return lengthFiltered;  
27
}
28