Revision eb7af729
Arduino code measures PWM up and PWM down time, and if U+D!= contant, we reject the signal
arduino/RadioBuggyMega/RadioBuggyMega.ino | ||
---|---|---|
3 | 3 |
* @author Haley Dalzell (haylee) |
4 | 4 |
* @author Zach Dawson (zachyzach) |
5 | 5 |
* @author Matt Sebek (msebek) |
6 |
*@author |
|
6 | 7 |
*/ |
7 | 8 |
#include "receiver.h" |
8 | 9 |
#include "brake.h" |
... | ... | |
93 | 94 |
static int steer_angle; |
94 | 95 |
|
95 | 96 |
void loop() { |
96 |
|
|
97 | 97 |
//if(filter_loop(&thr_state, rc_available[THR_INDEX])) { |
98 |
if(rc_available[THR_INDEX]) {
|
|
98 |
if(rc_available[AIL_INDEX]) {
|
|
99 | 99 |
watchdog_feed(); |
100 |
raw_angle = receiver_get_angle(THR_INDEX);
|
|
101 |
smoothed_angle = filter_loop(&thr_state, raw_angle);
|
|
102 |
steer_angle = convert_rc_to_steering(smoothed_angle);
|
|
100 |
raw_angle = receiver_get_angle(AIL_INDEX);
|
|
101 |
smoothed_angle = convert_rc_to_steering(raw_angle);
|
|
102 |
steer_angle = filter_loop(&ail_state, smoothed_angle);
|
|
103 | 103 |
steering_set(steer_angle); |
104 | 104 |
} |
105 | 105 |
|
106 | 106 |
//if(filter_loop(&ail_state, rc_available[AIL_INDEX])) { |
107 |
if(rc_available[AIL_INDEX]) {
|
|
107 |
if(rc_available[THR_INDEX]) {
|
|
108 | 108 |
watchdog_feed(); |
109 |
raw_thr = receiver_get_angle(AIL_INDEX);
|
|
110 |
smoothed_thr = filter_loop(&ail_state, raw_thr);
|
|
109 |
raw_thr = receiver_get_angle(THR_INDEX);
|
|
110 |
smoothed_thr = filter_loop(&thr_state, raw_thr);
|
|
111 | 111 |
// TODO make this code...less...something |
112 | 112 |
if(smoothed_thr < 90) { |
113 | 113 |
brake_drop(); |
arduino/RadioBuggyMega/filter.c | ||
---|---|---|
8 | 8 |
state->started = 0; |
9 | 9 |
} |
10 | 10 |
|
11 |
//<<<<<<< Updated upstream |
|
11 | 12 |
int filter_loop(struct filter_state *state, int val){ |
12 | 13 |
int i; |
13 | 14 |
// First initialization: on assumption val is never -1 |
14 | 15 |
if(!(state->started)){ |
15 | 16 |
for(i = 0; i < FILTER_LEN; i++){ |
16 | 17 |
state->prev_values[i] = val; |
18 |
//======= |
|
19 |
//#define SIZE 5 // the array size |
|
20 |
//static char started = 0; |
|
21 |
//static int last_val_sent; |
|
22 |
//static int values[SIZE]; |
|
23 |
//static int pos; |
|
24 |
// |
|
25 |
///** |
|
26 |
// * @brief Filters noise from raw RC readings |
|
27 |
// * |
|
28 |
// */ |
|
29 |
//int filter(int new_raw_reading){ |
|
30 |
// int i; |
|
31 |
// |
|
32 |
// // On Initialization, assume our "first" reading |
|
33 |
// // occurred N times, and is our history |
|
34 |
// if(!started){ |
|
35 |
// for(i = 0; i < SIZE; i++){ |
|
36 |
// values[i] = new_raw_reading; |
|
37 |
//>>>>>>> Stashed changes |
|
17 | 38 |
} |
18 | 39 |
state->pos = 0; // initialize the positions of the next number to be changed |
19 | 40 |
state->started = 1; |
20 | 41 |
return val; |
21 |
} |
|
42 |
} |
|
43 |
|
|
22 | 44 |
|
45 |
//<<<<<<< Updated upstream |
|
23 | 46 |
// NOT first initialization. |
24 | 47 |
state->prev_values[state->pos] = val; // replace the number in the array |
25 | 48 |
state->pos = (state->pos+1)%FILTER_LEN;// increment the pos number |
26 | 49 |
// calculate average of array |
50 |
//======= |
|
51 |
// |
|
52 |
// |
|
53 |
// values[pos] = val; // replace the number in the array |
|
54 |
// pos = (pos+1)%SIZE; // increment the pos number |
|
55 |
// |
|
56 |
// // calculate average of previous readings |
|
57 |
//>>>>>>> Stashed changes |
|
27 | 58 |
int sum = 0; |
28 | 59 |
for(i = 0; i < FILTER_LEN; i++){ |
29 | 60 |
sum += state->prev_values[i]; |
30 | 61 |
} |
62 |
//<<<<<<< Updated upstream |
|
31 | 63 |
int avg = sum/FILTER_LEN; |
32 | 64 |
// return. |
33 | 65 |
return avg; |
66 |
//======= |
|
67 |
// // Return the average. |
|
68 |
// return sum / SIZE; |
|
69 |
//>>>>>>> Stashed changes |
|
34 | 70 |
|
35 | 71 |
} |
arduino/RadioBuggyMega/filter.h | ||
---|---|---|
13 | 13 |
extern "C"{ |
14 | 14 |
#endif |
15 | 15 |
|
16 |
#define FILTER_LEN 5 // Averaging window size |
|
16 |
//<<<<<<< Updated upstream |
|
17 |
#define FILTER_LEN 10 // Averaging window size |
|
17 | 18 |
|
18 | 19 |
struct filter_state { |
19 | 20 |
int prev_values[FILTER_LEN]; |
... | ... | |
25 | 26 |
void filter_init(struct filter_state *state); |
26 | 27 |
|
27 | 28 |
int filter_loop(struct filter_state *state, int val); // returns the int that has been filtered |
29 |
//======= |
|
30 |
//// @param val The next raw reading |
|
31 |
//// @returns The next filtered reading |
|
32 |
//int filter(int new_raw_reading); |
|
33 |
// |
|
34 |
//>>>>>>> Stashed changes |
|
28 | 35 |
|
29 | 36 |
|
30 | 37 |
#ifdef __cplusplus |
arduino/RadioBuggyMega/receiver.c | ||
---|---|---|
20 | 20 |
#define THR_RIGHTMOST -1 |
21 | 21 |
#define THR_CENTERMOST -1 |
22 | 22 |
|
23 |
#define AIL_RECEIVER_PIN 3
|
|
24 |
#define AIL_RECEIVER_INT 1
|
|
23 |
#define AIL_RECEIVER_PIN 2
|
|
24 |
#define AIL_RECEIVER_INT 0
|
|
25 | 25 |
|
26 |
#define THR_RECEIVER_PIN 2 |
|
27 |
#define THR_RECEIVER_INT 0 |
|
26 |
#define THR_RECEIVER_PIN 3 |
|
27 |
#define THR_RECEIVER_INT 1 |
|
28 |
|
|
29 |
#define PWM_TIME 21800 |
|
30 |
#define PWM_THRESH 130 |
|
28 | 31 |
|
29 | 32 |
// Defined in receiver.h |
30 | 33 |
// #define THR_INDEX 0 |
31 | 34 |
// #define AIL_INDEX 1 |
32 | 35 |
|
33 | 36 |
// Note: arr[0] is thr, arr[1] is ail |
34 |
static volatile unsigned long start_time[2]; |
|
35 |
static volatile unsigned long rc_value[2]; |
|
36 | 37 |
|
38 |
static volatile unsigned long up_switch_time[2]; |
|
39 |
static volatile unsigned long down_switch_time[2]; |
|
40 |
static volatile unsigned long rc_value[2]; |
|
37 | 41 |
|
42 |
// NOTE THAT WE ARE ASSUMING: |
|
43 |
// Main Loop-length is shorter than PWM pulse length. |
|
44 |
// Otherwise, you could recieve stale values in your main loop. |
|
45 |
// Loop reading rc_values must set rc_available false after reading |
|
38 | 46 |
// When our code gets really busy this will become inaccurate, |
39 | 47 |
// (i believe since micros gets shifted a bit) but for |
40 | 48 |
// the current application its easy to understand and works very well |
... | ... | |
42 | 50 |
// TODO if this starts twitching, also micros has a resolution of 4us. |
43 | 51 |
static void receiver_on_ail_interrupt() { |
44 | 52 |
if(digitalRead(AIL_RECEIVER_PIN) == HIGH) { |
45 |
start_time[AIL_INDEX] = micros(); |
|
53 |
// High Received |
|
54 |
if((micros() - up_switch_time[AIL_INDEX] > PWM_TIME - PWM_THRESH) && |
|
55 |
(micros() - up_switch_time[AIL_INDEX] < PWM_TIME + PWM_THRESH)) { |
|
56 |
// TODO this prevent an instantaneous start-up error, where the |
|
57 |
// above condition is true, and up_switch_time |
|
58 |
if((down_switch_time[AIL_INDEX] > up_switch_time[AIL_INDEX]) && |
|
59 |
(rc_available[AIL_INDEX] == 0)) { |
|
60 |
rc_value[AIL_INDEX] = (down_switch_time[AIL_INDEX] - |
|
61 |
up_switch_time[AIL_INDEX]); |
|
62 |
rc_available[AIL_INDEX] = 1; |
|
63 |
if(rc_value[AIL_INDEX]/(PWM_TIME/10) >= 1 || rc_value[AIL_INDEX]/(PWM_TIME/21) == 0) |
|
64 |
rc_available[AIL_INDEX] == 0; |
|
65 |
} |
|
66 |
} |
|
67 |
up_switch_time[AIL_INDEX] = micros(); |
|
46 | 68 |
} else { |
47 |
if(start_time[AIL_INDEX] && (rc_available[AIL_INDEX] == 0)) { |
|
48 |
rc_value[AIL_INDEX] = (micros() - start_time[AIL_INDEX]); |
|
49 |
start_time[AIL_INDEX] = 0; |
|
50 |
rc_available[AIL_INDEX] = 1; |
|
69 |
// Low received |
|
70 |
if(up_switch_time[AIL_INDEX]) { |
|
71 |
down_switch_time[AIL_INDEX] = micros(); |
|
51 | 72 |
} |
52 | 73 |
} |
53 | 74 |
} |
54 | 75 |
|
55 | 76 |
static void receiver_on_thr_interrupt() { |
56 | 77 |
if(digitalRead(THR_RECEIVER_PIN) == HIGH) { |
57 |
start_time[THR_INDEX] = micros(); |
|
78 |
// High Received |
|
79 |
if((micros() - up_switch_time[THR_INDEX] > PWM_TIME - PWM_THRESH) && |
|
80 |
(micros() - up_switch_time[THR_INDEX] < PWM_TIME + PWM_THRESH)) { |
|
81 |
// TODO this prevent an instantaneous start-up error, where the |
|
82 |
// above condition is true, and up_switch_time |
|
83 |
if((down_switch_time[THR_INDEX] > up_switch_time[THR_INDEX]) && |
|
84 |
(rc_available[THR_INDEX] == 0)) { |
|
85 |
rc_value[THR_INDEX] = (down_switch_time[THR_INDEX] - |
|
86 |
up_switch_time[THR_INDEX]); |
|
87 |
rc_available[THR_INDEX] = 1; |
|
88 |
if(rc_value[THR_INDEX]/(PWM_TIME/10) >= 1 || rc_value[THR_INDEX]/(PWM_TIME/21) == 0) |
|
89 |
rc_available[THR_INDEX] == 0; |
|
90 |
} |
|
91 |
} |
|
92 |
up_switch_time[THR_INDEX] = micros(); |
|
58 | 93 |
} else { |
59 |
if(start_time[THR_INDEX] && (rc_available[THR_INDEX] == 0)) { |
|
60 |
rc_value[THR_INDEX] = (int)(micros() - start_time[THR_INDEX]); |
|
61 |
start_time[THR_INDEX] = 0; |
|
62 |
rc_available[THR_INDEX] = 1; |
|
94 |
// Low received |
|
95 |
if(up_switch_time[THR_INDEX]) { |
|
96 |
down_switch_time[THR_INDEX] = micros(); |
|
63 | 97 |
} |
64 | 98 |
} |
65 | 99 |
} |
66 | 100 |
|
67 | 101 |
// Returns error code |
68 | 102 |
int receiver_init() { |
103 |
up_switch_time[0] = 0; |
|
104 |
up_switch_time[1] = 0; |
|
105 |
down_switch_time[0] = 0; |
|
106 |
down_switch_time[1] = 0; |
|
69 | 107 |
attachInterrupt(THR_RECEIVER_INT, receiver_on_thr_interrupt, CHANGE); |
70 | 108 |
attachInterrupt(AIL_RECEIVER_INT, receiver_on_ail_interrupt, CHANGE); |
71 | 109 |
} |
... | ... | |
76 | 114 |
// TODO measure throttle positions. |
77 | 115 |
int receiver_get_angle(int index) { |
78 | 116 |
// Math to convert nThrottleIn to 0-180. |
79 |
int ret_val = (rc_value[index]-AIL_RIGHTMOST)*3/17; |
|
117 |
int ret_val = (int)(rc_value[index]-AIL_RIGHTMOST)*3/17;
|
|
80 | 118 |
rc_available[index] = 0; |
81 | 119 |
return ret_val; |
82 | 120 |
} |
121 |
|
|
122 |
|
|
123 |
|
|
124 |
|
arduino/RadioBuggyMega/receiver.h | ||
---|---|---|
35 | 35 |
#endif /* __cplusplus */ |
36 | 36 |
|
37 | 37 |
#endif /* _RECEIVER_H_ */ |
38 |
|
|
38 |
|
Also available in: Unified diff