robobuggy / arduino / RadioBuggyMega / RadioBuggyMega.ino @ 5ea63f0d
History | View | Annotate | Download (2.8 KB)
1 |
/** |
---|---|
2 |
* @file RadioBuggyMega.ino |
3 |
* @author Haley Dalzell (haylee) |
4 |
* @author Zach Dawson (zachyzach) |
5 |
* @author Matt Sebek (msebek) |
6 |
*@author |
7 |
*/ |
8 |
#include "receiver.h" |
9 |
#include "brake.h" |
10 |
#include "encoder.h" |
11 |
#include "watchdog.h" |
12 |
#include "filter.h" |
13 |
|
14 |
|
15 |
#define BRAKE_PIN 8 |
16 |
#define BRAKE_INDICATOR_PIN 5 |
17 |
|
18 |
#define ENCODER_PIN 7 |
19 |
|
20 |
#define STEERING_PIN 9 |
21 |
#define STEERING_CENTER 133 |
22 |
|
23 |
#define THR_PIN 2 |
24 |
#define AIL_PIN 3 |
25 |
|
26 |
#define XBEE_MSG_REC 4 |
27 |
|
28 |
#define XBEE_DANGER 12 |
29 |
|
30 |
#define TIME_THRESH 1000 |
31 |
|
32 |
unsigned long timer = 0L; |
33 |
static char data; // used to pass things into xbee |
34 |
static unsigned long last_time; |
35 |
|
36 |
// Initialize filter for |
37 |
struct filter_state ail_state; |
38 |
struct filter_state thr_state; |
39 |
|
40 |
#define LED_DANGER_PIN 12 |
41 |
|
42 |
enum STATE { START, RC_CON, RC_DC, BBB_CON }; |
43 |
|
44 |
// TODO: FIX IT WHEN IT STOPS FAILING. MAKE CODE BREAK BETTER |
45 |
|
46 |
void watchdog_fail(){ |
47 |
brake_drop(); |
48 |
Serial.println("Watchdog Fail! -------------------"); |
49 |
digitalWrite(LED_DANGER_PIN, HIGH); |
50 |
} |
51 |
|
52 |
|
53 |
|
54 |
void setup() { |
55 |
Serial.begin(9600); |
56 |
Serial1.begin(9600); |
57 |
|
58 |
//pinMode(XBEE_MSG_REC, OUTPUT); |
59 |
|
60 |
// Initialize Buggy |
61 |
// Pins 2 and 3: pin 2 is thr, pin 3 is ail |
62 |
receiver_init(); |
63 |
filter_init(&ail_state); |
64 |
filter_init(&thr_state); |
65 |
watchdog_init(TIME_THRESH, &watchdog_fail); |
66 |
brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN); |
67 |
steering_init(STEERING_PIN, 120, 133, 145); |
68 |
encoder_init(ENCODER_PIN); |
69 |
|
70 |
pinMode(LED_DANGER_PIN, OUTPUT); |
71 |
|
72 |
// Set up loop |
73 |
//last_time = millis(); |
74 |
} |
75 |
|
76 |
int convert_rc_to_steering(int rc_angle) { |
77 |
//Inverter for 2.4 GHz racecar received |
78 |
rc_angle = 180-rc_angle; |
79 |
int out = (rc_angle/4)+(90*3/4)+36; |
80 |
if(out < 100 || out > 155) { |
81 |
Serial.println("FAKFAKFAK SERVO OUT OF RANGE"); |
82 |
Serial.println(out); |
83 |
out = 125; |
84 |
} |
85 |
return out; |
86 |
} |
87 |
|
88 |
//code that keeps loop time constant each loop |
89 |
static int hz = 40; |
90 |
static int print_period = 1000 / hz; |
91 |
|
92 |
static int raw_angle; |
93 |
static int smoothed_angle; |
94 |
static int raw_thr; |
95 |
static int smoothed_thr; |
96 |
static int steer_angle; |
97 |
|
98 |
void loop() { |
99 |
//if(filter_loop(&thr_state, rc_available[THR_INDEX])) { |
100 |
if(rc_available[AIL_INDEX]) { |
101 |
watchdog_feed(); |
102 |
raw_angle = receiver_get_angle(AIL_INDEX); |
103 |
smoothed_angle = convert_rc_to_steering(raw_angle); |
104 |
steer_angle = filter_loop(&ail_state, smoothed_angle); |
105 |
steering_set(steer_angle); |
106 |
} |
107 |
|
108 |
//if(filter_loop(&ail_state, rc_available[AIL_INDEX])) { |
109 |
if(rc_available[THR_INDEX]) { |
110 |
watchdog_feed(); |
111 |
raw_thr = receiver_get_angle(THR_INDEX); |
112 |
smoothed_thr = filter_loop(&thr_state, raw_thr); |
113 |
// TODO make this code...less...something |
114 |
if(smoothed_thr < 70) { |
115 |
brake_drop(); |
116 |
} else { |
117 |
brake_raise(); |
118 |
} |
119 |
} |
120 |
|
121 |
// Loop |
122 |
watchdog_loop(); |
123 |
encoder_loop(); |
124 |
|
125 |
// If timer expired, then do ROS things |
126 |
if((last_time - millis()) > 0) { |
127 |
Serial.print(steer_angle); |
128 |
Serial.print(" "); |
129 |
Serial.println(encoder_get_count()); |
130 |
} |
131 |
|
132 |
} |