Project

General

Profile

Statistics
| Branch: | Revision:

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
}