From 6b942ebc8fd2b1848db06ef56bbfea6b7c2da462 Mon Sep 17 00:00:00 2001 From: Matt Sebek Date: Sat, 6 Sep 2014 21:49:07 -0400 Subject: [PATCH] Added last year's brake code As far as I can tell, this is the code that is currently on buggy. Speak now/soon, or forever hold your peace. --- arduino_src/RCBuggyBackup.ino | 132 +++++++++++++++++++++++++++++++++ arduino_src/RadioBuggyMega.ino | 87 ++++++++++++++++++++++ arduino_src/brake.c | 33 +++++++++ arduino_src/brake.h | 19 +++++ arduino_src/encoder.c | 36 +++++++++ arduino_src/encoder.h | 15 ++++ arduino_src/receiver.c | 76 +++++++++++++++++++ arduino_src/receiver.h | 38 ++++++++++ arduino_src/steering.c | 27 +++++++ arduino_src/xbee.c | 72 ++++++++++++++++++ 10 files changed, 535 insertions(+) create mode 100644 arduino_src/RCBuggyBackup.ino create mode 100644 arduino_src/RadioBuggyMega.ino create mode 100644 arduino_src/brake.c create mode 100644 arduino_src/brake.h create mode 100644 arduino_src/encoder.c create mode 100644 arduino_src/encoder.h create mode 100644 arduino_src/receiver.c create mode 100644 arduino_src/receiver.h create mode 100644 arduino_src/steering.c create mode 100644 arduino_src/xbee.c diff --git a/arduino_src/RCBuggyBackup.ino b/arduino_src/RCBuggyBackup.ino new file mode 100644 index 00000000..36942d71 --- /dev/null +++ b/arduino_src/RCBuggyBackup.ino @@ -0,0 +1,132 @@ +/* + Makes Arduino receive input from xbee and parse data. + + The circuit: + * RX is digital pin 2 (connect to TX of XBee) + * TX is digital pin 3 (connect to RX of XBee) + + */ + +//#include +#include + +#define BRAKE_PIN 8 +#define BRAKE_INDICATOR_PIN 5 + +#define SERVO_PIN 9 +#define STEERING_CENTER 133 + +#define XBEE_MSG_REC 4 + +Servo myservo; // create servo object to control a servo + +unsigned long timer = 0L; +char data; +String message; +char intbuf[32]; +int brake = 0; +int steeringAngle = STEERING_CENTER; +int pingPong = 1; +int startfound = 0; +int midfound = 0; +int endfound = 1; + +//code that keeps loop time constant each loop +int hz = 40; +int print_period = 1000 / hz; +unsigned long last_time = 0; + +void setup() { + Serial.begin(9600); + Serial1.begin(9600); + + pinMode(XBEE_MSG_REC, OUTPUT); + + // initialize the brake with brake pin and led brake pin + brake_init(brakePin, brakeLedPin); + + + myservo.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object + myservo.write(STEEING_CENTER); +} + +void loop() { + + // receive and parse message from xbee + if(Serial1.available() > 0) { + + timer = millis(); + + // read message from xbee + data = Serial1.read(); + + // parse message data + if (data == 'A') { + startfound = 1; + midfound = 0; + endfound = 0; + message = ""; + } + else if (data == 'B') { + startfound = 0; + midfound = 1; + endfound = 0; + } + else if (data == 'C') { + startfound = 0; + midfound = 0; + endfound = 1; + } + else if (startfound) { + message = message + data; + } + else if (midfound) { + if(data == '1') + brake = 1; + else + brake = 0; + message.toCharArray(intbuf, sizeof(intbuf)); + steeringAngle = atoi(intbuf); + } + + // flop external LED everytime message is recieved + if ( pingPong == 0 ) { + digitalWrite(4, LOW); + } + else { + digitalWrite(4, HIGH); + } + + pingPong = 1 - pingPong; + + } // end receive message + + // brake if it has been greater than 3 seconds since we last got a message + if( (millis() - timer) > 5000L ) { + digitalWrite(8, 0); + digitalWrite(5, HIGH); + exit(1); + } + + if((millis() - timer) > 2000L) { + digitalWrite(12, HIGH); + } + else { + digitalWrite(12, LOW); + } + + // make brake LED light up if brakes should be down + if( brake == 0 ) { + digitalWrite(5, HIGH); + } + else { + digitalWrite(5, LOW); + } + + // send parsed signal to brakes and servo + digitalWrite(8, brake); + + // sets the servo position + myservo.write(steeringAngle); +} + diff --git a/arduino_src/RadioBuggyMega.ino b/arduino_src/RadioBuggyMega.ino new file mode 100644 index 00000000..d958fba1 --- /dev/null +++ b/arduino_src/RadioBuggyMega.ino @@ -0,0 +1,87 @@ +/** + * @file RadioBuggyMega.ino + * @author Haley Dalzell (haylee) + * @author Zach Dawson (zachyzach) + * @author Matt Sebek (msebek) + */ +#include + +#define BRAKE_PIN 8 +#define BRAKE_INDICATOR_PIN 5 + +#define ENCODER_PIN 7 + +#define STEERING_PIN 9 +#define STEERING_CENTER 133 + +#define THR_PIN 2 +#define AIL_PIN 3 + +#define XBEE_MSG_REC 4 + +#define XBEE_DANGER 12 + +unsigned long timer = 0L; +static char data; // used to pass things into xbee +int brake = 0; + + +enum STATE { START, RC_CON, RC_DC, BBB_CON }; + +void setup() { + Serial.begin(9600); + Serial1.begin(9600); + + pinMode(XBEE_MSG_REC, OUTPUT); + + // Initialize Buggy + // Pins 2 and 3: pin 2 is thr, pin 3 is ail + receiver_init(); + brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN); + steering_init(STEERING_PIN); + encoder_init(ENCODER_PIN); + + // Set up loop + last_time = millis(); +} + +int convert_rc_to_steering(int rc_angle) { + int out = (input/4)+(90*3/4)+39; + if(out < 105 || out > 160) { + Serial.println("FAKFAKFAK SERVO OUT OF RANGE"); + Serial.println(out); + out = 129; + } +} + +//code that keeps loop time constant each loop +static int hz = 40; +static int print_period = 1000 / hz; +static unsigned long last_time = 0; + +static int rc_angle; +static int rc_thr; +void loop() { + + if(rc_available[THR_INDEX]) { + rc_angle = receiver_get_angle(THR_INDEX); + } + + steering_set(convert_rc_to_steering(rc_angle)); + + if(rc_available[AIL_INDEX]) { + rc_thr receiver_get_angle(THR_PIN); + } + + // Loop + encoder_loop(); + xbee_loop(); + + // If timer expired, then do ROS things + if((last_time - millis()) > 0) { + Serial.println("Loop!"); + + + } + +} diff --git a/arduino_src/brake.c b/arduino_src/brake.c new file mode 100644 index 00000000..c8b46b80 --- /dev/null +++ b/arduino_src/brake.c @@ -0,0 +1,33 @@ +/** + * @file brake.c + * @author Audrey Yeoh (ayeoh) + * @author Matt Sebek (msebek) + */ +#include +#include "brake.h" + +static int brake_pin; +// Indicator LED pin number +static int indicator_pin; + +void brake_init(int brakePin, int indicatorLed) { + pinMode(brake_pin, OUTPUT); + pinMode(indicatorLed, OUTPUT); + brake_pin = brakePin; + indicator_pin = indicatorLed; +} + +// Note: High-voltage raises the brake. +// Raises the brake +// Do not call before brake_init +void brake_raise() { + digitalWrite(brake_pin, HIGH); + digitalWrite(indicator_pin, LOW); +} + +// Drops the brake +// Do not call before brake_init +void brake_drop() { + digitalWrite(brake_pin, LOW); + digitalWrite(indicator_pin, HIGH); +} diff --git a/arduino_src/brake.h b/arduino_src/brake.h new file mode 100644 index 00000000..c5727a8f --- /dev/null +++ b/arduino_src/brake.h @@ -0,0 +1,19 @@ +/** + * @file brake.h + * @author Audrey Yeoh (ayeoh) + * @author Matt Sebek (msebek) + * + * Initializes, raises, drops, and gets-state of + * the brakes. + * + */ +#ifndef _BRAKE_H_ +#define _BRAKE_H_ + + void brake_init(int brakePin, int indicatorLedPin); + + void brake_raise(); + + void brake_drop(); + +#endif diff --git a/arduino_src/encoder.c b/arduino_src/encoder.c new file mode 100644 index 00000000..4676b3ac --- /dev/null +++ b/arduino_src/encoder.c @@ -0,0 +1,36 @@ +/** + * @file encoder.c + * @author Audrey Yeoh (ayeoh) + * @author Matt Sebek (msebek0 + */ + +static int input_pin; + +static int count=0; +// 1 if was high, 0 if was low +static int state=0; + +void encoder_init(int encoder_pin) { + input_pin = encoder_pin; + pinMode(input_pin, INPUT); +} + +// Every X ms, publish. (or perish!!) +void encoder_publish() { + if (last_time < (millis() - print_period)) { + Serial.println(count); + last_time = millis(); + } +} + +int encoder_get_count() { + return count; +} + +// Lightweight, checks low-pri encoder loop +void encoder_loop(){ + if (state != digital_read(input_pin)){ + count++; + state = !state; + } +} diff --git a/arduino_src/encoder.h b/arduino_src/encoder.h new file mode 100644 index 00000000..bed4f432 --- /dev/null +++ b/arduino_src/encoder.h @@ -0,0 +1,15 @@ +/** + * @file encoder.h + * @author Audrey Yeoh (ayeoh) + * @author Matt Sebek (msebek0 + */ + +void encoder_init(int encoder_pin); + +// Every X ms, publish. +void encoder_publish(); + +int encoder_get_count(); + +// Lightweight, checks low-pri encoder loop +void encoder_loop(); diff --git a/arduino_src/receiver.c b/arduino_src/receiver.c new file mode 100644 index 00000000..2a780650 --- /dev/null +++ b/arduino_src/receiver.c @@ -0,0 +1,76 @@ +/** + * @file receiver.cpp + * @brief Contains Code for dealing with RC Receiver + */ +//#include +#include +#include "receiver.h" + + +#define AIL_LEFTMOST 2000 +#define AIL_RIGHTMOST 980 +#define AIL_CENTERMOST 1480 + +#define THR_LEFTMOST -1 +#define THR_RIGHTMOST -1 +#define THR_CENTERMOST -1 + +#define AIL_RECEIVER_PIN 3 +#define AIL_RECEIVER_INT 1 + +#define THR_RECEIVER_PIN 2 +#define THR_RECEIVER_INT 0 + +//#define THR_INDEX 0 +//#define AIL_INDEX 1 + +// Note: arr[0] is thr, arr[1] is ail +static volatile int start_time[2]; +static volatile int rc_value[2]; + + +// When our code gets really busy this will become inaccurate, +// (i believe since micros gets shifted a bit) but for +// the current application its easy to understand and works very well +// TODO if things start twitching, move to using registers directly. +static void receiver_on_ail_interrupt() { + if(digitalRead(AIL_RECEIVER_PIN) == HIGH) { + start_time[AIL_INDEX] = micros(); + } else { + if(start_time[AIL_INDEX] && (rc_available[AIL_INDEX] == 0)) { + rc_value[AIL_INDEX] = (int)(micros() - start_time[AIL_INDEX]); + start_time[AIL_INDEX] = 0; + rc_available[AIL_INDEX] = 1; + } + } +} + +static void receiver_on_thr_interrupt() { + if(digitalRead(THR_RECEIVER_PIN) == HIGH) { + start_time[THR_INDEX] = micros(); + } else { + if(start_time[THR_INDEX] && (rc_available[THR_INDEX] == 0)) { + rc_value[THR_INDEX] = (int)(micros() - start_time[THR_INDEX]); + start_time[THR_INDEX] = 0; + rc_available[THR_INDEX] = 1; + } + } +} + +// Returns error code +int receiver_init() { + attachInterrupt(THR_RECEIVER_INT, receiver_on_thr_interrupt, CHANGE); + attachInterrupt(AIL_RECEIVER_INT, receiver_on_ail_interrupt, CHANGE); +} + + +// Index = 0 to check thr, index = 1 to check +// Returns 0 to 180, with 90 being center. +// TODO measure throttle positions. +int receiver_get_angle(int index) { + // Math to convert nThrottleIn to 0-180. + int ret_val = (rc_value[index]-AIL_RIGHTMOST)*3/17; + rc_available[index] = 0; + return ret_val; +} + diff --git a/arduino_src/receiver.h b/arduino_src/receiver.h new file mode 100644 index 00000000..154b2ba0 --- /dev/null +++ b/arduino_src/receiver.h @@ -0,0 +1,38 @@ +/** + * @file receiver.h + * @brief Headers for Receiver + * DOGE mooooooo + * doge COW! + */ + +#ifndef _RECEIVER_H_ +#define _RECEIVER_H_ + +#ifdef __cplusplus +extern "C"{ +#endif + + #define THR_INDEX 0 + #define AIL_INDEX 1 + + // To check if new value available, check the + // value contained in this, using the correct + // index. + volatile char rc_available[2]; + + // if this is true, we are connected. + volatile char rc_connected; + + int receiver_init(); + + // Zero for throttle position, 1 for ail pos + // returns an angle strictly between 0 and 180, + // with 90 being the center position. + int receiver_get_angle(int int_pin); + +#ifdef __cplusplus +} // extern "C" +#endif /* __cplusplus */ + +#endif /* _RECEIVER_H_ */ + diff --git a/arduino_src/steering.c b/arduino_src/steering.c new file mode 100644 index 00000000..9f89b69a --- /dev/null +++ b/arduino_src/steering.c @@ -0,0 +1,27 @@ +/** + * @file steering.c + * @brief Steering! + * @author Haley Dalzell + * @author Zach Dawson + * @author Matt Sebek + */ + +Servo steer; // create servo object to control a servo +static int s_angle = STEERING_CENTER; +static int s_left, s_center, s_right; + +void steering_init(int SERVO_PIN, int left, int center, int right) { + steer.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object + s_left = left; + s_center = center; + s_right = right; +} + +void steering_set(int servo_value) { + if(servo_value < s_left) { + s_angle = s_left; + } else if(servo_value > s_right) { + s_angle = s_right; + } + steer.write(s_angle); +} diff --git a/arduino_src/xbee.c b/arduino_src/xbee.c new file mode 100644 index 00000000..3431d1af --- /dev/null +++ b/arduino_src/xbee.c @@ -0,0 +1,72 @@ +/** + * Code relating to xbee + * + */ + +static int xbee_brake; +static int xbee_angle; +static int xbee_connected; + +String message; +char intbuf[32]; + +int pingPong = 1; +int startfound = 0; +int midfound = 0; +int endfound = 1; + +void xbee_init() { + + +} + +void xbee_loop() { + // receive and parse message from xbee + if(Serial1.available() > 0) { + + timer = millis(); + + // read message from xbee + data = Serial1.read(); + + // parse message data + if (data == 'A') { + startfound = 1; + midfound = 0; + endfound = 0; + message = ""; + } + else if (data == 'B') { + startfound = 0; + midfound = 1; + endfound = 0; + } + else if (data == 'C') { + startfound = 0; + midfound = 0; + endfound = 1; + } + else if (startfound) { + message = message + data; + } + else if (midfound) { + if(data == '1') + brake = 1; + else + brake = 0; + message.toCharArray(intbuf, sizeof(intbuf)); + steeringAngle = atoi(intbuf); + } + + // flop external LED everytime message is recieved + if ( pingPong == 0 ) { + digitalWrite(4, LOW); + } + else { + digitalWrite(4, HIGH); + } + + pingPong = 1 - pingPong; + + } // end receive message +}