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