Project

General

Profile

Statistics
| Branch: | Revision:

robobuggy / arduino / RadioBuggyMega / RadioBuggyMega.ino @ 5ea63f0d

History | View | Annotate | Download (2.8 KB)

1
/**
2
 * @file RadioBuggyMega.ino
3
 * @author Haley Dalzell (haylee)
4
 * @author Zach Dawson (zachyzach)
5
 * @author Matt Sebek (msebek)
6
  *@author 
7
 */
8
#include "receiver.h"
9
#include "brake.h"
10
#include "encoder.h"
11
#include "watchdog.h"
12
#include "filter.h"
13

    
14

    
15
#define BRAKE_PIN 8
16
#define BRAKE_INDICATOR_PIN 5
17

    
18
#define ENCODER_PIN 7
19

    
20
#define STEERING_PIN 9
21
#define STEERING_CENTER 133
22

    
23
#define THR_PIN 2
24
#define AIL_PIN 3
25

    
26
#define XBEE_MSG_REC 4
27

    
28
#define XBEE_DANGER 12
29

    
30
#define TIME_THRESH 1000
31

    
32
unsigned long timer = 0L;
33
static char data; // used to pass things into xbee
34
static unsigned long last_time;
35

    
36
// Initialize filter for
37
struct filter_state ail_state;
38
struct filter_state thr_state;
39

    
40
#define LED_DANGER_PIN 12
41

    
42
enum STATE { START, RC_CON, RC_DC, BBB_CON };
43

    
44
// TODO: FIX IT WHEN IT STOPS FAILING. MAKE CODE BREAK BETTER
45

    
46
void watchdog_fail(){
47
 brake_drop();
48
 Serial.println("Watchdog Fail! -------------------");
49
 digitalWrite(LED_DANGER_PIN, HIGH);
50
}
51

    
52

    
53

    
54
void setup()  {
55
  Serial.begin(9600);
56
  Serial1.begin(9600);
57

    
58
  //pinMode(XBEE_MSG_REC, OUTPUT);
59

    
60
  // Initialize Buggy
61
  // Pins 2 and 3: pin 2 is thr, pin 3 is ail
62
  receiver_init();
63
  filter_init(&ail_state);
64
  filter_init(&thr_state);
65
  watchdog_init(TIME_THRESH, &watchdog_fail);
66
  brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN);
67
  steering_init(STEERING_PIN, 120, 133, 145);
68
  encoder_init(ENCODER_PIN);
69

    
70
  pinMode(LED_DANGER_PIN, OUTPUT);
71

    
72
  // Set up loop
73
  //last_time = millis();
74
}
75

    
76
int convert_rc_to_steering(int rc_angle) {
77
  //Inverter for 2.4 GHz racecar received
78
  rc_angle = 180-rc_angle;
79
  int out = (rc_angle/4)+(90*3/4)+36;
80
  if(out < 100 || out > 155) {
81
    Serial.println("FAKFAKFAK SERVO OUT OF RANGE");
82
    Serial.println(out);
83
    out = 125;
84
  }
85
  return out;
86
}
87

    
88
//code that keeps loop time constant each loop
89
static int hz = 40;
90
static int print_period = 1000 / hz;
91

    
92
static int raw_angle;
93
static int smoothed_angle;
94
static int raw_thr;
95
static int smoothed_thr;
96
static int steer_angle;
97

    
98
void loop() {
99
  //if(filter_loop(&thr_state, rc_available[THR_INDEX])) {
100
  if(rc_available[AIL_INDEX]) {
101
    watchdog_feed();
102
    raw_angle = receiver_get_angle(AIL_INDEX);
103
    smoothed_angle = convert_rc_to_steering(raw_angle);
104
    steer_angle = filter_loop(&ail_state, smoothed_angle);
105
    steering_set(steer_angle);
106
  }
107

    
108
  //if(filter_loop(&ail_state, rc_available[AIL_INDEX])) {
109
  if(rc_available[THR_INDEX]) {
110
    watchdog_feed();
111
    raw_thr = receiver_get_angle(THR_INDEX);
112
    smoothed_thr = filter_loop(&thr_state, raw_thr);
113
    // TODO make this code...less...something
114
    if(smoothed_thr < 70) {
115
      brake_drop();
116
    } else {
117
      brake_raise();
118
    }
119
  }
120

    
121
  // Loop
122
  watchdog_loop();
123
  encoder_loop();
124

    
125
  // If timer expired, then do ROS things
126
  if((last_time - millis()) > 0) {
127
    Serial.print(steer_angle);
128
    Serial.print("    ");
129
    Serial.println(encoder_get_count());
130
  }
131

    
132
}