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