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