Revision e6c4b5e0
Fixed receiver int should've been u_long, and filtering over rc_avail, not results
arduino/RadioBuggyMega/RadioBuggyMega.ino | ||
---|---|---|
32 | 32 |
static char data; // used to pass things into xbee |
33 | 33 |
static unsigned long last_time; |
34 | 34 |
|
35 |
// Initialize filter for
|
|
35 |
// Initialize filter for |
|
36 | 36 |
struct filter_state ail_state; |
37 | 37 |
struct filter_state thr_state; |
38 | 38 |
|
... | ... | |
43 | 43 |
// TODO: FIX IT WHEN IT STOPS FAILING. MAKE CODE BREAK BETTER |
44 | 44 |
|
45 | 45 |
void watchdog_fail(){ |
46 |
brake_drop();
|
|
46 |
brake_drop(); |
|
47 | 47 |
Serial.println("Watchdog Fail! -------------------"); |
48 | 48 |
digitalWrite(LED_DANGER_PIN, HIGH); |
49 | 49 |
} |
... | ... | |
65 | 65 |
brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN); |
66 | 66 |
steering_init(STEERING_PIN, 120, 133, 145); |
67 | 67 |
encoder_init(ENCODER_PIN); |
68 |
|
|
68 |
|
|
69 | 69 |
pinMode(LED_DANGER_PIN, OUTPUT); |
70 | 70 |
|
71 | 71 |
// Set up loop |
... | ... | |
86 | 86 |
static int hz = 40; |
87 | 87 |
static int print_period = 1000 / hz; |
88 | 88 |
|
89 |
static int rc_angle; |
|
90 |
static int rc_thr; |
|
89 |
static int raw_angle; |
|
90 |
static int smoothed_angle; |
|
91 |
static int raw_thr; |
|
92 |
static int smoothed_thr; |
|
91 | 93 |
static int steer_angle; |
92 | 94 |
|
93 | 95 |
void loop() { |
94 | 96 |
|
95 |
if(filter_loop(&thr_state, rc_available[THR_INDEX])) { |
|
97 |
//if(filter_loop(&thr_state, rc_available[THR_INDEX])) { |
|
98 |
if(rc_available[THR_INDEX]) { |
|
96 | 99 |
watchdog_feed(); |
97 |
rc_angle = receiver_get_angle(THR_INDEX); |
|
98 |
steer_angle = convert_rc_to_steering(rc_angle); |
|
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); |
|
99 | 103 |
steering_set(steer_angle); |
100 | 104 |
} |
101 |
|
|
102 |
if(filter_loop(&ail_state, rc_available[AIL_INDEX])) { |
|
105 |
|
|
106 |
//if(filter_loop(&ail_state, rc_available[AIL_INDEX])) { |
|
107 |
if(rc_available[AIL_INDEX]) { |
|
103 | 108 |
watchdog_feed(); |
104 |
rc_thr = receiver_get_angle(AIL_INDEX); |
|
109 |
raw_thr = receiver_get_angle(AIL_INDEX); |
|
110 |
smoothed_thr = filter_loop(&ail_state, raw_thr); |
|
105 | 111 |
// TODO make this code...less...something |
106 |
if(rc_thr < 90) {
|
|
107 |
brake_drop();
|
|
112 |
if(smoothed_thr < 90) {
|
|
113 |
brake_drop(); |
|
108 | 114 |
} else { |
109 |
brake_raise();
|
|
115 |
brake_raise(); |
|
110 | 116 |
} |
111 | 117 |
} |
112 |
|
|
118 |
|
|
113 | 119 |
// Loop |
114 | 120 |
watchdog_loop(); |
115 | 121 |
encoder_loop(); |
116 |
//xbee_loop(); |
|
117 | 122 |
|
118 | 123 |
// If timer expired, then do ROS things |
119 | 124 |
if((last_time - millis()) > 0) { |
arduino/RadioBuggyMega/receiver.c | ||
---|---|---|
1 | 1 |
/** |
2 |
* @file receiver.cpp
|
|
2 |
* @file receiver.c |
|
3 | 3 |
* @brief Contains Code for dealing with RC Receiver |
4 | 4 |
* NOTE: AIL and THR are flipped. |
5 | 5 |
* AIL is on pin 2 |
6 | 6 |
* THR is on pin 3 |
7 |
* |
|
8 |
* @author Matt Sebek (msebek) |
|
9 |
* @author Zach Dawson (zsd) |
|
7 | 10 |
*/ |
8 |
//#include <stdint.h> |
|
9 | 11 |
#include <Arduino.h> |
10 | 12 |
#include "receiver.h" |
11 | 13 |
|
... | ... | |
24 | 26 |
#define THR_RECEIVER_PIN 2 |
25 | 27 |
#define THR_RECEIVER_INT 0 |
26 | 28 |
|
27 |
//#define THR_INDEX 0 |
|
28 |
//#define AIL_INDEX 1 |
|
29 |
// Defined in receiver.h |
|
30 |
// #define THR_INDEX 0 |
|
31 |
// #define AIL_INDEX 1 |
|
29 | 32 |
|
30 | 33 |
// Note: arr[0] is thr, arr[1] is ail |
31 |
static volatile int start_time[2];
|
|
32 |
static volatile int rc_value[2];
|
|
34 |
static volatile unsigned long start_time[2];
|
|
35 |
static volatile unsigned long rc_value[2];
|
|
33 | 36 |
|
34 | 37 |
|
35 | 38 |
// When our code gets really busy this will become inaccurate, |
36 | 39 |
// (i believe since micros gets shifted a bit) but for |
37 | 40 |
// the current application its easy to understand and works very well |
38 | 41 |
// TODO if things start twitching, move to using registers directly. |
42 |
// TODO if this starts twitching, also micros has a resolution of 4us. |
|
39 | 43 |
static void receiver_on_ail_interrupt() { |
40 | 44 |
if(digitalRead(AIL_RECEIVER_PIN) == HIGH) { |
41 | 45 |
start_time[AIL_INDEX] = micros(); |
42 | 46 |
} else { |
43 | 47 |
if(start_time[AIL_INDEX] && (rc_available[AIL_INDEX] == 0)) { |
44 |
rc_value[AIL_INDEX] = (int)(micros() - start_time[AIL_INDEX]);
|
|
48 |
rc_value[AIL_INDEX] = (micros() - start_time[AIL_INDEX]); |
|
45 | 49 |
start_time[AIL_INDEX] = 0; |
46 | 50 |
rc_available[AIL_INDEX] = 1; |
47 | 51 |
} |
... | ... | |
76 | 80 |
rc_available[index] = 0; |
77 | 81 |
return ret_val; |
78 | 82 |
} |
79 |
|
Also available in: Unified diff