Revision 0c873a67
Fixed up all the buggy code.
arduino/InterruptSingleChannel/InterruptSingleChannel.ino | ||
---|---|---|
1 |
// First Example in a series of posts illustrating reading an RC Receiver with |
|
2 |
// micro controller interrupts. |
|
3 |
// |
|
4 |
// Subsequent posts will provide enhancements required for real world operation |
|
5 |
// in high speed applications with multiple inputs. |
|
6 |
// |
|
7 |
// http://rcarduino.blogspot.com/ |
|
8 |
// |
|
9 |
// Posts in the series will be titled - How To Read an RC Receiver With A Microcontroller |
|
10 |
|
|
11 |
// See also http://rcarduino.blogspot.co.uk/2012/04/how-to-read-multiple-rc-channels-draft.html |
|
12 |
|
|
13 |
// Include the servo library |
|
1 |
/** |
|
2 |
* @file InterruptSingleChannel.ino |
|
3 |
* @brief Read in RC, then write value to servo. |
|
4 |
*/ |
|
14 | 5 |
#include <Servo.h> |
15 | 6 |
#include "receiver.h" |
16 | 7 |
|
17 |
Servo myservo; |
|
18 |
// Center is 90 |
|
8 |
#define STEER_CENTER 129 |
|
9 |
|
|
10 |
// 1/RC_STEER_DAMPER times RC_VALUE (0-180) |
|
11 |
#define RC_STEER_DAMPER 4 |
|
12 |
Servo steering_servo; |
|
19 | 13 |
int pos = 0; |
14 |
|
|
20 | 15 |
// Note the following mapping is used between pins and interrupt numbers: |
21 | 16 |
//Board int.0 int.1 int.2 int.3 int.4 int.5 |
22 | 17 |
//Uno, Ethernet 2 3 |
... | ... | |
27 | 22 |
void setup() |
28 | 23 |
{ |
29 | 24 |
receiver_init(); |
30 |
myservo.attach(9); // attaches the servo on pin 9 to the servo object
|
|
25 |
steering_servo.attach(9); // attaches the servo on pin 9 to the servo object
|
|
31 | 26 |
Serial.begin(9600); |
32 | 27 |
} |
33 | 28 |
|
34 | 29 |
void loop() |
35 | 30 |
{ |
36 | 31 |
static int input = 128; |
37 |
if(receiver_new()) {
|
|
38 |
input = receiver_get_angle(); |
|
32 |
if(rc_available[THR_INDEX]) {
|
|
33 |
input = receiver_get_angle(THR_INDEX);
|
|
39 | 34 |
} |
40 | 35 |
|
41 |
//myservo.write(133+(input-128)/20);
|
|
36 |
//steering_servo.write(133+(input-128)/20);
|
|
42 | 37 |
// I decided 133 is center |
43 | 38 |
// Note that 4 is a damping factor. |
44 | 39 |
// 43 |
... | ... | |
49 | 44 |
out = 129; |
50 | 45 |
} |
51 | 46 |
Serial.println(out); |
52 |
myservo.write(out);
|
|
47 |
steering_servo.write(out);
|
|
53 | 48 |
} |
54 |
|
|
55 |
|
arduino/InterruptSingleChannel/receiver.c | ||
---|---|---|
21 | 21 |
#define THR_RECEIVER_PIN 2 |
22 | 22 |
#define THR_RECEIVER_INT 0 |
23 | 23 |
|
24 |
//#define THR_INDEX 0 |
|
25 |
//#define AIL_INDEX 1 |
|
26 |
|
|
24 | 27 |
// Note: arr[0] is thr, arr[1] is ail |
25 |
static volatile static int start_time[2]; |
|
26 |
static volatile bool rc_available[2]; |
|
28 |
static volatile int start_time[2]; |
|
27 | 29 |
static volatile int rc_value[2]; |
28 |
static volatile bool rc_connected[2]; |
|
29 | 30 |
|
30 |
// Returns error code |
|
31 |
int receiver_init() { |
|
32 |
attachInterrupt(THR_RECEIVER_INT, receiver_on_thr_interrupt, CHANGE); |
|
33 |
attachInterrupt(AIL_RECEIVER_INT, receiver_on_ail_interrupt, CHANGE); |
|
34 |
} |
|
35 | 31 |
|
36 | 32 |
// When our code gets really busy this will become inaccurate, |
37 | 33 |
// (i believe since micros gets shifted a bit) but for |
38 | 34 |
// the current application its easy to understand and works very well |
39 |
// Write code twice, look suspicious. Write code three times, refactor |
|
40 |
// Aileron Interrupt |
|
41 | 35 |
// TODO if things start twitching, move to using registers directly. |
42 |
void receiver_on_ail_interrupt() { |
|
36 |
static void receiver_on_ail_interrupt() {
|
|
43 | 37 |
if(digitalRead(AIL_RECEIVER_PIN) == HIGH) { |
44 |
start_time[1] = micros();
|
|
38 |
start_time[AIL_INDEX] = micros();
|
|
45 | 39 |
} else { |
46 |
if(start_time[1] && (rc_available[1] == 0)) {
|
|
47 |
rc_value[1] = (int)(micros() - ulStartPeriod);
|
|
48 |
start_time[1] = 0;
|
|
49 |
rc_available[1] = 1;
|
|
40 |
if(start_time[AIL_INDEX] && (rc_available[AIL_INDEX] == 0)) {
|
|
41 |
rc_value[AIL_INDEX] = (int)(micros() - start_time[AIL_INDEX]);
|
|
42 |
start_time[AIL_INDEX] = 0;
|
|
43 |
rc_available[AIL_INDEX] = 1;
|
|
50 | 44 |
} |
51 | 45 |
} |
52 | 46 |
} |
53 | 47 |
|
54 |
void receiver_on_thr_interrupt() { |
|
48 |
static void receiver_on_thr_interrupt() {
|
|
55 | 49 |
if(digitalRead(THR_RECEIVER_PIN) == HIGH) { |
56 |
start_time[0] = micros();
|
|
50 |
start_time[THR_INDEX] = micros();
|
|
57 | 51 |
} else { |
58 |
if(start_time[0] && (rc_available[0] == 0)) {
|
|
59 |
rc_value[0] = (int)(micros() - ulStartPeriod);
|
|
60 |
start_time[0] = 0;
|
|
61 |
rc_available[0] = 1;
|
|
52 |
if(start_time[THR_INDEX] && (rc_available[THR_INDEX] == 0)) {
|
|
53 |
rc_value[THR_INDEX] = (int)(micros() - start_time[THR_INDEX]);
|
|
54 |
start_time[THR_INDEX] = 0;
|
|
55 |
rc_available[THR_INDEX] = 1;
|
|
62 | 56 |
} |
63 | 57 |
} |
64 | 58 |
} |
65 | 59 |
|
66 |
// Returns the value, bounded by proper limits |
|
67 |
int receiver_avail(int i) { |
|
68 |
return ; |
|
60 |
// Returns error code |
|
61 |
int receiver_init() { |
|
62 |
attachInterrupt(THR_RECEIVER_INT, receiver_on_thr_interrupt, CHANGE); |
|
63 |
attachInterrupt(AIL_RECEIVER_INT, receiver_on_ail_interrupt, CHANGE); |
|
69 | 64 |
} |
70 | 65 |
|
71 |
// Gets the angle without a dead zone |
|
72 |
// Returns a value between 0 and 255, with no
|
|
73 |
// movement being 128.
|
|
74 |
// Maps this into ~0 to ~180, 90 being straight.
|
|
75 |
int receiver_get_angle(int i) { |
|
66 |
|
|
67 |
// Index = 0 to check thr, index = 1 to check
|
|
68 |
// Returns 0 to 180, with 90 being center.
|
|
69 |
// TODO measure throttle positions.
|
|
70 |
int receiver_get_angle(int index) {
|
|
76 | 71 |
// Math to convert nThrottleIn to 0-180. |
77 |
bNewThrottleSignal = 0; |
|
78 |
return ((nThrottleIn-RIGHTMOST)*3)/17; |
|
72 |
int ret_val = (rc_value[index]-AIL_RIGHTMOST)*3/17; |
|
73 |
rc_available[index] = 0; |
|
74 |
return ret_val; |
|
79 | 75 |
} |
80 | 76 |
|
arduino/InterruptSingleChannel/receiver.h | ||
---|---|---|
12 | 12 |
extern "C"{ |
13 | 13 |
#endif |
14 | 14 |
|
15 |
int receiver_init(); |
|
15 |
#define THR_INDEX 0 |
|
16 |
#define AIL_INDEX 1 |
|
17 |
|
|
18 |
// To check if new value available, check the |
|
19 |
// value contained in this, using the correct |
|
20 |
// index. |
|
21 |
volatile char rc_available[2]; |
|
16 | 22 |
|
17 |
void receiver_on_interrupt(int int_pin); |
|
23 |
// if this is true, we are connected. |
|
24 |
volatile char rc_connected; |
|
25 |
|
|
26 |
int receiver_init(); |
|
18 | 27 |
|
19 |
// Gets the angle without a dead zone |
|
20 |
// Returns a value between 0 and 255, with no |
|
21 |
// movement being 128. |
|
28 |
// Zero for throttle position, 1 for ail pos |
|
29 |
// returns an angle strictly between 0 and 180, |
|
30 |
// with 90 being the center position. |
|
31 |
int receiver_get_angle(int int_pin); |
|
22 | 32 |
|
23 | 33 |
#ifdef __cplusplus |
24 | 34 |
} // extern "C" |
arduino/RCBuggyBackup.ino | ||
---|---|---|
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 |
*/ |
|
9 |
|
|
10 |
//#include <SoftwareSerial.h> |
|
11 |
#include <Servo.h> |
|
12 |
|
|
13 |
#define BRAKE_PIN 8 |
|
14 |
#define BRAKE_INDICATOR_PIN 5 |
|
15 |
|
|
16 |
#define SERVO_PIN 9 |
|
17 |
#define STEERING_CENTER 133 |
|
18 |
|
|
19 |
#define XBEE_MSG_REC 4 |
|
20 |
|
|
21 |
Servo myservo; // create servo object to control a servo |
|
22 |
|
|
23 |
unsigned long timer = 0L; |
|
24 |
char data; |
|
25 |
String message; |
|
26 |
char intbuf[32]; |
|
27 |
int brake = 0; |
|
28 |
int steeringAngle = STEERING_CENTER; |
|
29 |
int pingPong = 1; |
|
30 |
int startfound = 0; |
|
31 |
int midfound = 0; |
|
32 |
int endfound = 1; |
|
33 |
|
|
34 |
//code that keeps loop time constant each loop |
|
35 |
int hz = 40; |
|
36 |
int print_period = 1000 / hz; |
|
37 |
unsigned long last_time = 0; |
|
38 |
|
|
39 |
void setup() { |
|
40 |
Serial.begin(9600); |
|
41 |
Serial1.begin(9600); |
|
42 |
|
|
43 |
pinMode(XBEE_MSG_REC, OUTPUT); |
|
44 |
|
|
45 |
// initialize the brake with brake pin and led brake pin |
|
46 |
brake_init(brakePin, brakeLedPin); |
|
47 |
|
|
48 |
|
|
49 |
myservo.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object |
|
50 |
myservo.write(STEEING_CENTER); |
|
51 |
} |
|
52 |
|
|
53 |
void loop() { |
|
54 |
|
|
55 |
// receive and parse message from xbee |
|
56 |
if(Serial1.available() > 0) { |
|
57 |
|
|
58 |
timer = millis(); |
|
59 |
|
|
60 |
// read message from xbee |
|
61 |
data = Serial1.read(); |
|
62 |
|
|
63 |
// parse message data |
|
64 |
if (data == 'A') { |
|
65 |
startfound = 1; |
|
66 |
midfound = 0; |
|
67 |
endfound = 0; |
|
68 |
message = ""; |
|
69 |
} |
|
70 |
else if (data == 'B') { |
|
71 |
startfound = 0; |
|
72 |
midfound = 1; |
|
73 |
endfound = 0; |
|
74 |
} |
|
75 |
else if (data == 'C') { |
|
76 |
startfound = 0; |
|
77 |
midfound = 0; |
|
78 |
endfound = 1; |
|
79 |
} |
|
80 |
else if (startfound) { |
|
81 |
message = message + data; |
|
82 |
} |
|
83 |
else if (midfound) { |
|
84 |
if(data == '1') |
|
85 |
brake = 1; |
|
86 |
else |
|
87 |
brake = 0; |
|
88 |
message.toCharArray(intbuf, sizeof(intbuf)); |
|
89 |
steeringAngle = atoi(intbuf); |
|
90 |
} |
|
91 |
|
|
92 |
// flop external LED everytime message is recieved |
|
93 |
if ( pingPong == 0 ) { |
|
94 |
digitalWrite(4, LOW); |
|
95 |
} |
|
96 |
else { |
|
97 |
digitalWrite(4, HIGH); |
|
98 |
} |
|
99 |
|
|
100 |
pingPong = 1 - pingPong; |
|
101 |
|
|
102 |
} // end receive message |
|
103 |
|
|
104 |
// brake if it has been greater than 3 seconds since we last got a message |
|
105 |
if( (millis() - timer) > 5000L ) { |
|
106 |
digitalWrite(8, 0); |
|
107 |
digitalWrite(5, HIGH); |
|
108 |
exit(1); |
|
109 |
} |
|
110 |
|
|
111 |
if((millis() - timer) > 2000L) { |
|
112 |
digitalWrite(12, HIGH); |
|
113 |
} |
|
114 |
else { |
|
115 |
digitalWrite(12, LOW); |
|
116 |
} |
|
117 |
|
|
118 |
// make brake LED light up if brakes should be down |
|
119 |
if( brake == 0 ) { |
|
120 |
digitalWrite(5, HIGH); |
|
121 |
} |
|
122 |
else { |
|
123 |
digitalWrite(5, LOW); |
|
124 |
} |
|
125 |
|
|
126 |
// send parsed signal to brakes and servo |
|
127 |
digitalWrite(8, brake); |
|
128 |
|
|
129 |
// sets the servo position |
|
130 |
myservo.write(steeringAngle); |
|
131 |
} |
|
132 |
|
arduino/RCbuggyMega/RCbuggyMega.ino | ||
---|---|---|
1 | 1 |
/* |
2 | 2 |
Makes Arduino receive input from xbee and parse data. |
3 |
|
|
3 |
|
|
4 | 4 |
The circuit: |
5 | 5 |
* RX is digital pin 2 (connect to TX of XBee) |
6 | 6 |
* TX is digital pin 3 (connect to RX of XBee) |
7 |
|
|
7 |
|
|
8 | 8 |
*/ |
9 | 9 |
|
10 | 10 |
//#include <SoftwareSerial.h> |
11 |
#include <Servo.h> |
|
11 |
#include <Servo.h> |
|
12 |
|
|
13 |
#define BRAKE_PIN 8 |
|
14 |
#define BRAKE_INDICATOR_PIN 5 |
|
12 | 15 |
|
13 |
Servo myservo; // create servo object to control a servo |
|
16 |
#define SERVO_PIN 9 |
|
17 |
#define STEERING_CENTER 133 |
|
18 |
|
|
19 |
Servo myservo; // create servo object to control a servo |
|
14 | 20 |
|
15 | 21 |
//SoftwareSerial xbee(2, 3); // RX, TX |
16 | 22 |
unsigned long timer = 0L; |
... | ... | |
18 | 24 |
String message; |
19 | 25 |
char intbuf[32]; |
20 | 26 |
int brake = 0; |
21 |
int steeringAngle = 135;
|
|
27 |
int steeringAngle = STEERING_CENTER;
|
|
22 | 28 |
int pingPong = 1; |
23 | 29 |
int startfound = 0; |
24 | 30 |
int midfound = 0; |
25 | 31 |
int endfound = 1; |
26 | 32 |
|
27 |
int brakePin = 8; |
|
28 |
int brakeLedPin = 5; |
|
29 | 33 |
|
30 | 34 |
void setup() { |
31 | 35 |
Serial.begin(9600); |
32 |
//Serial.println( "Arduino started sending bytes via XBee" ); |
|
33 | 36 |
Serial1.begin(9600); |
34 |
|
|
37 |
|
|
35 | 38 |
pinMode(4, OUTPUT); |
36 |
pinMode(5, OUTPUT); |
|
37 |
pinMode(8, OUTPUT);
|
|
38 |
|
|
39 |
brake_init(brakePin, brakeLedPin); // initialize the brake with brake pin and led brake pin |
|
40 |
|
|
41 |
myservo.attach(9); // attaches the servo on pin 9 to the servo object
|
|
42 |
myservo.write(133);
|
|
39 |
|
|
40 |
// initialize the brake with brake pin and led brake pin
|
|
41 |
brake_init(brakePin, brakeLedPin); |
|
42 |
|
|
43 |
|
|
44 |
myservo.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object
|
|
45 |
myservo.write(STEEING_CENTER);
|
|
43 | 46 |
} |
44 | 47 |
|
45 | 48 |
void loop() { |
46 | 49 |
|
47 | 50 |
// receive and parse message from xbee |
48 |
if(Serial1.available() > 0) {
|
|
51 |
if(Serial1.available() > 0) { |
|
49 | 52 |
|
50 | 53 |
timer = millis(); |
51 | 54 |
|
... | ... | |
79 | 82 |
brake = 0; |
80 | 83 |
message.toCharArray(intbuf, sizeof(intbuf)); |
81 | 84 |
steeringAngle = atoi(intbuf); |
82 |
}
|
|
85 |
} |
|
83 | 86 |
|
84 | 87 |
// flop external LED everytime message is recieved |
85 | 88 |
if ( pingPong == 0 ) { |
86 | 89 |
digitalWrite(4, LOW); |
87 |
}
|
|
90 |
} |
|
88 | 91 |
else { |
89 | 92 |
digitalWrite(4, HIGH); |
90 | 93 |
} |
... | ... | |
110 | 113 |
// make brake LED light up if brakes should be down |
111 | 114 |
if( brake == 0 ) { |
112 | 115 |
digitalWrite(5, HIGH); |
113 |
}
|
|
116 |
} |
|
114 | 117 |
else { |
115 | 118 |
digitalWrite(5, LOW); |
116 | 119 |
} |
arduino/RCbuggyMega/brake.c | ||
---|---|---|
1 |
/** |
|
2 |
* @file brake.c |
|
3 |
* @author Audrey Yeoh (ayeoh) |
|
4 |
* @author Matt Sebek (msebek) |
|
5 |
*/ |
|
1 | 6 |
#include <Arduino.h> |
2 | 7 |
#include "brake.h" |
3 | 8 |
|
4 |
static int pin; |
|
9 |
static int brake_pin; |
|
10 |
// Indicator LED pin number |
|
11 |
static int indicator_pin; |
|
5 | 12 |
|
6 |
void brake_init(int brakePin, int ledPin) { |
|
7 |
brake = brakePin; |
|
8 |
led = ledPin; |
|
13 |
void brake_init(int brakePin, int indicatorLed) { |
|
14 |
pinMode(brake_pin, OUTPUT); |
|
15 |
pinMode(indicatorLed, OUTPUT); |
|
16 |
brake_pin = brakePin; |
|
17 |
indicator_pin = indicatorLed; |
|
9 | 18 |
} |
10 | 19 |
|
11 |
void raiseBrake(){ |
|
12 |
digitalWrite(brake, 1); |
|
13 |
digitalWrite(led, LOW); |
|
20 |
// Note: High-voltage raises the brake. |
|
21 |
// Raises the brake |
|
22 |
// Do not call before brake_init |
|
23 |
void brake_raise() { |
|
24 |
digitalWrite(brake_pin, HIGH); |
|
25 |
digitalWrite(indicator_pin, LOW); |
|
14 | 26 |
} |
15 | 27 |
|
16 |
void dropBrake(){ |
|
17 |
digitalWrite(brake, 0); |
|
18 |
digitalWrite(led, HIGH); |
|
28 |
// Drops the brake |
|
29 |
// Do not call before brake_init |
|
30 |
void brake_drop() { |
|
31 |
digitalWrite(brake_pin, LOW); |
|
32 |
digitalWrite(indicator_pin, HIGH); |
|
19 | 33 |
} |
20 |
|
|
21 |
|
arduino/RCbuggyMega/brake.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file brake.h |
|
3 |
* @author Audrey Yeoh (ayeoh) |
|
4 |
* @author Matt Sebek (msebek) |
|
5 |
* |
|
6 |
* Initializes, raises, drops, and gets-state of |
|
7 |
* the brakes. |
|
8 |
* |
|
9 |
*/ |
|
1 | 10 |
#ifndef _BRAKE_H_ |
2 | 11 |
#define _BRAKE_H_ |
3 | 12 |
|
4 |
void brake_init(int brakePin, int ledPin); |
|
5 |
|
|
6 |
void raiseBrake(); |
|
7 |
|
|
8 |
void dropBrake(); |
|
13 |
void brake_init(int brakePin, int indicatorLedPin); |
|
9 | 14 |
|
10 |
#endif |
|
15 |
void brake_raise(); |
|
16 |
|
|
17 |
void brake_drop(); |
|
18 |
|
|
19 |
#endif |
arduino/RCbuggyMega/xbee.c | ||
---|---|---|
1 |
/** |
|
2 |
* Code relating to |
arduino/RadioBuggyMega/RadioBuggyMega.ino | ||
---|---|---|
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 |
|
|
11 |
#define BRAKE_PIN 8 |
|
12 |
#define BRAKE_INDICATOR_PIN 5 |
|
13 |
|
|
14 |
#define ENCODER_PIN 7 |
|
15 |
|
|
16 |
#define STEERING_PIN 9 |
|
17 |
#define STEERING_CENTER 133 |
|
18 |
|
|
19 |
#define THR_PIN 2 |
|
20 |
#define AIL_PIN 3 |
|
21 |
|
|
22 |
#define XBEE_MSG_REC 4 |
|
23 |
|
|
24 |
#define XBEE_DANGER 12 |
|
25 |
|
|
26 |
unsigned long timer = 0L; |
|
27 |
static char data; // used to pass things into xbee |
|
28 |
static unsigned long last_time; |
|
29 |
|
|
30 |
enum STATE { START, RC_CON, RC_DC, BBB_CON }; |
|
31 |
|
|
32 |
void setup() { |
|
33 |
Serial.begin(9600); |
|
34 |
Serial1.begin(9600); |
|
35 |
|
|
36 |
//pinMode(XBEE_MSG_REC, OUTPUT); |
|
37 |
|
|
38 |
// Initialize Buggy |
|
39 |
// Pins 2 and 3: pin 2 is thr, pin 3 is ail |
|
40 |
receiver_init(); |
|
41 |
brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN); |
|
42 |
steering_init(STEERING_PIN, 120, 133, 145); |
|
43 |
encoder_init(ENCODER_PIN); |
|
44 |
|
|
45 |
// Set up loop |
|
46 |
//last_time = millis(); |
|
47 |
} |
|
48 |
|
|
49 |
int convert_rc_to_steering(int rc_angle) { |
|
50 |
int out = (rc_angle/4)+(90*3/4)+39; |
|
51 |
if(out < 105 || out > 160) { |
|
52 |
Serial.println("FAKFAKFAK SERVO OUT OF RANGE"); |
|
53 |
Serial.println(out); |
|
54 |
out = 129; |
|
55 |
} |
|
56 |
return out; |
|
57 |
} |
|
58 |
|
|
59 |
//code that keeps loop time constant each loop |
|
60 |
static int hz = 40; |
|
61 |
static int print_period = 1000 / hz; |
|
62 |
|
|
63 |
static int rc_angle; |
|
64 |
static int rc_thr; |
|
65 |
static int steer_angle; |
|
66 |
|
|
67 |
void loop() { |
|
68 |
|
|
69 |
if(rc_available[THR_INDEX]) { |
|
70 |
rc_angle = receiver_get_angle(THR_INDEX); |
|
71 |
steer_angle = convert_rc_to_steering(rc_angle); |
|
72 |
steering_set(steer_angle); |
|
73 |
} |
|
74 |
|
|
75 |
if(rc_available[AIL_INDEX]) { |
|
76 |
rc_thr = receiver_get_angle(AIL_INDEX); |
|
77 |
// TODO make this code...less...something |
|
78 |
if(rc_thr < 90) { |
|
79 |
brake_drop(); |
|
80 |
} else { |
|
81 |
brake_raise(); |
|
82 |
} |
|
83 |
} |
|
84 |
|
|
85 |
// Loop |
|
86 |
encoder_loop(); |
|
87 |
//xbee_loop(); |
|
88 |
|
|
89 |
// If timer expired, then do ROS things |
|
90 |
if((last_time - millis()) > 0) { |
|
91 |
Serial.println(encoder_get_count()); |
|
92 |
} |
|
93 |
|
|
94 |
} |
arduino/RadioBuggyMega/brake.c | ||
---|---|---|
1 |
/** |
|
2 |
* @file brake.c |
|
3 |
* @author Audrey Yeoh (ayeoh) |
|
4 |
* @author Matt Sebek (msebek) |
|
5 |
*/ |
|
6 |
#include <Arduino.h> |
|
7 |
#include "brake.h" |
|
8 |
|
|
9 |
static int brake_pin; |
|
10 |
// Indicator LED pin number |
|
11 |
static int indicator_pin; |
|
12 |
|
|
13 |
void brake_init(int brakePin, int indicatorLed) { |
|
14 |
pinMode(brakePin, OUTPUT); |
|
15 |
pinMode(indicatorLed, OUTPUT); |
|
16 |
brake_pin = brakePin; |
|
17 |
indicator_pin = indicatorLed; |
|
18 |
} |
|
19 |
|
|
20 |
// Note: High-voltage raises the brake. |
|
21 |
// Raises the brake |
|
22 |
// Do not call before brake_init |
|
23 |
void brake_raise() { |
|
24 |
digitalWrite(brake_pin, HIGH); |
|
25 |
digitalWrite(indicator_pin, LOW); |
|
26 |
} |
|
27 |
|
|
28 |
// Drops the brake |
|
29 |
// Do not call before brake_init |
|
30 |
void brake_drop() { |
|
31 |
digitalWrite(brake_pin, LOW); |
|
32 |
digitalWrite(indicator_pin, HIGH); |
|
33 |
} |
arduino/RadioBuggyMega/brake.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file brake.h |
|
3 |
* @author Audrey Yeoh (ayeoh) |
|
4 |
* @author Matt Sebek (msebek) |
|
5 |
* |
|
6 |
* Initializes, raises, drops, and gets-state of |
|
7 |
* the brakes. |
|
8 |
* |
|
9 |
*/ |
|
10 |
#ifndef _BRAKE_H_ |
|
11 |
#define _BRAKE_H_ |
|
12 |
|
|
13 |
#ifdef __cplusplus |
|
14 |
extern "C"{ |
|
15 |
#endif |
|
16 |
|
|
17 |
|
|
18 |
void brake_init(int brakePin, int indicatorLedPin); |
|
19 |
|
|
20 |
void brake_raise(); |
|
21 |
|
|
22 |
void brake_drop(); |
|
23 |
|
|
24 |
#ifdef __cplusplus |
|
25 |
} // extern "C" |
|
26 |
#endif /* __cplusplus */ |
|
27 |
|
|
28 |
#endif /* _BRAKE_H_ */ |
arduino/RadioBuggyMega/encoder.c | ||
---|---|---|
1 |
/** |
|
2 |
* @file encoder.c |
|
3 |
*/ |
|
4 |
#include <Arduino.h> |
|
5 |
#include "encoder.h" |
|
6 |
|
|
7 |
static int enc_pin = 1; |
|
8 |
static int count = 0; |
|
9 |
static int state = 1; // 1 if was high, 0 if was low |
|
10 |
|
|
11 |
|
|
12 |
void encoder_init(int encoder_pin) { |
|
13 |
pinMode(encoder_pin, INPUT); |
|
14 |
enc_pin = encoder_pin; |
|
15 |
} |
|
16 |
|
|
17 |
int encoder_get_count() { |
|
18 |
return count; |
|
19 |
} |
|
20 |
|
|
21 |
// Lightweight, checks low-pri encoder loop |
|
22 |
void encoder_loop() { |
|
23 |
if (state != digitalRead(enc_pin)){ |
|
24 |
count++; |
|
25 |
state = !state; |
|
26 |
} |
|
27 |
} |
|
28 |
|
|
29 |
|
|
30 |
|
arduino/RadioBuggyMega/encoder.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file encoder.h |
|
3 |
* @author Audrey Yeoh (ayeoh) |
|
4 |
* @author Matt Sebek (msebek) |
|
5 |
*/ |
|
6 |
#ifndef _ENCODER_H_ |
|
7 |
#define _ENCODER_H_ |
|
8 |
|
|
9 |
#ifdef __cplusplus |
|
10 |
extern "C"{ |
|
11 |
#endif |
|
12 |
|
|
13 |
|
|
14 |
void encoder_init(int encoder_pin); |
|
15 |
|
|
16 |
// Every X ms, publish. |
|
17 |
void encoder_publish(); |
|
18 |
|
|
19 |
int encoder_get_count(); |
|
20 |
|
|
21 |
// Lightweight, checks low-pri encoder loop |
|
22 |
void encoder_loop(); |
|
23 |
|
|
24 |
#ifdef __cplusplus |
|
25 |
} // extern "C" |
|
26 |
#endif /* __cplusplus */ |
|
27 |
|
|
28 |
#endif /* _ENCODER_H_ */ |
arduino/RadioBuggyMega/receiver.c | ||
---|---|---|
1 |
/** |
|
2 |
* @file receiver.cpp |
|
3 |
* @brief Contains Code for dealing with RC Receiver |
|
4 |
* NOTE: AIL and THR are flipped. |
|
5 |
* AIL is on pin 2 |
|
6 |
* THR is on pin 3 |
|
7 |
*/ |
|
8 |
//#include <stdint.h> |
|
9 |
#include <Arduino.h> |
|
10 |
#include "receiver.h" |
|
11 |
|
|
12 |
|
|
13 |
#define AIL_LEFTMOST 2000 |
|
14 |
#define AIL_RIGHTMOST 980 |
|
15 |
#define AIL_CENTERMOST 1480 |
|
16 |
|
|
17 |
#define THR_LEFTMOST -1 |
|
18 |
#define THR_RIGHTMOST -1 |
|
19 |
#define THR_CENTERMOST -1 |
|
20 |
|
|
21 |
#define AIL_RECEIVER_PIN 3 |
|
22 |
#define AIL_RECEIVER_INT 1 |
|
23 |
|
|
24 |
#define THR_RECEIVER_PIN 2 |
|
25 |
#define THR_RECEIVER_INT 0 |
|
26 |
|
|
27 |
//#define THR_INDEX 0 |
|
28 |
//#define AIL_INDEX 1 |
|
29 |
|
|
30 |
// Note: arr[0] is thr, arr[1] is ail |
|
31 |
static volatile int start_time[2]; |
|
32 |
static volatile int rc_value[2]; |
|
33 |
|
|
34 |
|
|
35 |
// When our code gets really busy this will become inaccurate, |
|
36 |
// (i believe since micros gets shifted a bit) but for |
|
37 |
// the current application its easy to understand and works very well |
|
38 |
// TODO if things start twitching, move to using registers directly. |
|
39 |
static void receiver_on_ail_interrupt() { |
|
40 |
if(digitalRead(AIL_RECEIVER_PIN) == HIGH) { |
|
41 |
start_time[AIL_INDEX] = micros(); |
|
42 |
} else { |
|
43 |
if(start_time[AIL_INDEX] && (rc_available[AIL_INDEX] == 0)) { |
|
44 |
rc_value[AIL_INDEX] = (int)(micros() - start_time[AIL_INDEX]); |
|
45 |
start_time[AIL_INDEX] = 0; |
|
46 |
rc_available[AIL_INDEX] = 1; |
|
47 |
} |
|
48 |
} |
|
49 |
} |
|
50 |
|
|
51 |
static void receiver_on_thr_interrupt() { |
|
52 |
if(digitalRead(THR_RECEIVER_PIN) == HIGH) { |
|
53 |
start_time[THR_INDEX] = micros(); |
|
54 |
} else { |
|
55 |
if(start_time[THR_INDEX] && (rc_available[THR_INDEX] == 0)) { |
|
56 |
rc_value[THR_INDEX] = (int)(micros() - start_time[THR_INDEX]); |
|
57 |
start_time[THR_INDEX] = 0; |
|
58 |
rc_available[THR_INDEX] = 1; |
|
59 |
} |
|
60 |
} |
|
61 |
} |
|
62 |
|
|
63 |
// Returns error code |
|
64 |
int receiver_init() { |
|
65 |
attachInterrupt(THR_RECEIVER_INT, receiver_on_thr_interrupt, CHANGE); |
|
66 |
attachInterrupt(AIL_RECEIVER_INT, receiver_on_ail_interrupt, CHANGE); |
|
67 |
} |
|
68 |
|
|
69 |
|
|
70 |
// Index = 0 to check thr, index = 1 to check |
|
71 |
// Returns 0 to 180, with 90 being center. |
|
72 |
// TODO measure throttle positions. |
|
73 |
int receiver_get_angle(int index) { |
|
74 |
// Math to convert nThrottleIn to 0-180. |
|
75 |
int ret_val = (rc_value[index]-AIL_RIGHTMOST)*3/17; |
|
76 |
rc_available[index] = 0; |
|
77 |
return ret_val; |
|
78 |
} |
|
79 |
|
arduino/RadioBuggyMega/receiver.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file receiver.h |
|
3 |
* @brief Headers for Receiver |
|
4 |
* DOGE mooooooo |
|
5 |
* doge COW! |
|
6 |
*/ |
|
7 |
|
|
8 |
#ifndef _RECEIVER_H_ |
|
9 |
#define _RECEIVER_H_ |
|
10 |
|
|
11 |
#ifdef __cplusplus |
|
12 |
extern "C"{ |
|
13 |
#endif |
|
14 |
|
|
15 |
#define THR_INDEX 0 |
|
16 |
#define AIL_INDEX 1 |
|
17 |
|
|
18 |
// To check if new value available, check the |
|
19 |
// value contained in this, using the correct |
|
20 |
// index. |
|
21 |
volatile int rc_available[2]; |
|
22 |
|
|
23 |
// if this is true, we are connected. |
|
24 |
volatile char rc_connected; |
|
25 |
|
|
26 |
int receiver_init(); |
|
27 |
|
|
28 |
// Zero for throttle position, 1 for ail pos |
|
29 |
// returns an angle strictly between 0 and 180, |
|
30 |
// with 90 being the center position. |
|
31 |
int receiver_get_angle(int int_pin); |
|
32 |
|
|
33 |
#ifdef __cplusplus |
|
34 |
} // extern "C" |
|
35 |
#endif /* __cplusplus */ |
|
36 |
|
|
37 |
#endif /* _RECEIVER_H_ */ |
|
38 |
|
arduino/RadioBuggyMega/steering.ino | ||
---|---|---|
1 |
/** |
|
2 |
* @file steering.c |
|
3 |
* @brief Steering! |
|
4 |
* @author Haley Dalzell |
|
5 |
* @author Zach Dawson |
|
6 |
* @author Matt Sebek |
|
7 |
*/ |
|
8 |
#include <Servo.h> |
|
9 |
|
|
10 |
Servo steer; // create servo object to control a servo |
|
11 |
static int s_angle = STEERING_CENTER; |
|
12 |
static int s_left, s_center, s_right; |
|
13 |
|
|
14 |
void steering_init(int SERVO_PIN, int left, int center, int right) { |
|
15 |
steer.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object |
|
16 |
s_left = left; |
|
17 |
s_center = center; |
|
18 |
s_right = right; |
|
19 |
} |
|
20 |
|
|
21 |
void steering_set(int servo_value) { |
|
22 |
/* if(servo_value < s_left) { |
|
23 |
s_angle = s_left; |
|
24 |
servo_value = s_left; |
|
25 |
} else if(servo_value > s_right) { |
|
26 |
s_angle = s_right; |
|
27 |
servo_value = s_right; |
|
28 |
} */ |
|
29 |
steer.write(servo_value); |
|
30 |
} |
arduino/xbee.ino | ||
---|---|---|
1 |
/** |
|
2 |
* Code relating to xbee |
|
3 |
* |
|
4 |
*/ |
|
5 |
|
|
6 |
static int xbee_brake; |
|
7 |
static int xbee_angle; |
|
8 |
static int xbee_connected; |
|
9 |
|
|
10 |
String message; |
|
11 |
char intbuf[32]; |
|
12 |
|
|
13 |
int pingPong = 1; |
|
14 |
int startfound = 0; |
|
15 |
int midfound = 0; |
|
16 |
int endfound = 1; |
|
17 |
|
|
18 |
void xbee_init() { |
|
19 |
|
|
20 |
|
|
21 |
} |
|
22 |
|
|
23 |
void xbee_loop() { |
|
24 |
// receive and parse message from xbee |
|
25 |
if(Serial1.available() > 0) { |
|
26 |
|
|
27 |
timer = millis(); |
|
28 |
|
|
29 |
// read message from xbee |
|
30 |
data = Serial1.read(); |
|
31 |
|
|
32 |
// parse message data |
|
33 |
if (data == 'A') { |
|
34 |
startfound = 1; |
|
35 |
midfound = 0; |
|
36 |
endfound = 0; |
|
37 |
message = ""; |
|
38 |
} |
|
39 |
else if (data == 'B') { |
|
40 |
startfound = 0; |
|
41 |
midfound = 1; |
|
42 |
endfound = 0; |
|
43 |
} |
|
44 |
else if (data == 'C') { |
|
45 |
startfound = 0; |
|
46 |
midfound = 0; |
|
47 |
endfound = 1; |
|
48 |
} |
|
49 |
else if (startfound) { |
|
50 |
message = message + data; |
|
51 |
} |
|
52 |
else if (midfound) { |
|
53 |
if(data == '1') |
|
54 |
brake = 1; |
|
55 |
else |
|
56 |
brake = 0; |
|
57 |
message.toCharArray(intbuf, sizeof(intbuf)); |
|
58 |
steeringAngle = atoi(intbuf); |
|
59 |
} |
|
60 |
|
|
61 |
// flop external LED everytime message is recieved |
|
62 |
if ( pingPong == 0 ) { |
|
63 |
digitalWrite(4, LOW); |
|
64 |
} |
|
65 |
else { |
|
66 |
digitalWrite(4, HIGH); |
|
67 |
} |
|
68 |
|
|
69 |
pingPong = 1 - pingPong; |
|
70 |
|
|
71 |
} // end receive message |
|
72 |
} |
Also available in: Unified diff