Project

General

Profile

Statistics
| Branch: | Revision:

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
}