robobuggy / arduino / RadioBuggyMega / RadioBuggyMega.ino @ 1ba00e63
History | View | Annotate | Download (2.25 KB)
1 | 0c873a67 | msebek | /** |
---|---|---|---|
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 | 1ba00e63 | unknown | #include "watchdog.h" |
11 | 0c873a67 | msebek | |
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 | 1ba00e63 | unknown | #define TIME_THRESH 1000 |
28 | |||
29 | 0c873a67 | msebek | unsigned long timer = 0L; |
30 | static char data; // used to pass things into xbee |
||
31 | static unsigned long last_time; |
||
32 | |||
33 | 1ba00e63 | unknown | |
34 | #define LED_DANGER_PIN 12 |
||
35 | |||
36 | 0c873a67 | msebek | enum STATE { START, RC_CON, RC_DC, BBB_CON }; |
37 | |||
38 | 1ba00e63 | unknown | // 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 | 0c873a67 | msebek | 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 | 1ba00e63 | unknown | watchdog_init(TIME_THRESH, &watchdog_fail); |
56 | 0c873a67 | msebek | brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN); |
57 | steering_init(STEERING_PIN, 120, 133, 145); |
||
58 | encoder_init(ENCODER_PIN); |
||
59 | 1ba00e63 | unknown | |
60 | pinMode(LED_DANGER_PIN, OUTPUT); |
||
61 | 0c873a67 | msebek | |
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 | 1ba00e63 | unknown | watchdog_feed(); |
88 | 0c873a67 | msebek | 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 | 1ba00e63 | unknown | watchdog_feed(); |
95 | 0c873a67 | msebek | 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 | 1ba00e63 | unknown | watchdog_loop(); |
106 | 0c873a67 | msebek | 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 | } |