Project

General

Profile

Statistics
| Branch: | Revision:

robobuggy / arduino / RCbuggyMega / RCbuggyMega.ino @ 6331ce9f

History | View | Annotate | Download (1.63 KB)

1
/*
2
  Makes Arduino receive input from xbee and parse data.
3

    
4
 The circuit:
5
 * RX is digital pin 2 (connect to TX of XBee)
6
 * TX is digital pin 3 (connect to RX of XBee)
7
 
8
 * changes made:
9
 * @author: Auds (ayeoh)
10

    
11
 */
12

    
13
#include <SoftwareSerial.h>
14
#include <Servo.h>
15
#include "brake.h"
16
#include "xbee.h"
17

    
18
#define BRAKE_PIN 8
19
#define BRAKE_INDICATOR_PIN 5
20
#define XBEE_LED_PIN 4
21

    
22
#define SERVO_PIN 9
23

    
24
unsigned long timer = 0L;
25
char data;
26

    
27

    
28
Servo myservo;  // create servo object to control a servo
29

    
30
void setup()  {
31
  Serial.begin(9600);
32
  Serial1.begin(9600);
33

    
34
  xbee_init(XBEE_LED_PIN);
35

    
36
  // initialize the brake with brake pin and led brake pin
37
  brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN);
38

    
39

    
40
  myservo.attach(SERVO_PIN);  // attaches the servo on pin 9 to the servo object
41
  myservo.write(getSteeringAngle());
42
}
43

    
44
void loop()  {
45

    
46
  // receive and parse message from xbee
47
  if(Serial1.available() > 0) {
48

    
49
    timer = millis();
50

    
51
    // read message from xbee
52
    data = Serial1.read();
53

    
54
    parse(data);
55

    
56
    // flop external LED everytime message is recieved
57
    debugMessage();
58

    
59
  } // end receive message
60

    
61
  // brake if it has been greater than 3 seconds since we last got a message
62
  if( (millis() - timer) > 5000L ) {
63
    brake_drop();
64
    exit(1);
65
  }
66

    
67
  /* Stuff I was told not to care about
68
  if((millis() - timer) > 2000L) {
69
    digitalWrite(12, HIGH);
70
  }
71
  else {
72
    digitalWrite(12, LOW);
73
  }*/
74
  
75
  int brake = getBrake();
76
  // make brake LED light up if brakes should be down
77
  if( brake == 0 ) {
78
    brake_drop();
79
  }else {
80
    brake_raise();
81
  }
82

    
83
  int steeringAngle = getSteeringAngle();
84
 
85
 // sets the servo position
86
  myservo.write(steeringAngle);
87
}
88