Skip to content

Instantly share code, notes, and snippets.

@apetrone
Last active September 4, 2015 01:44
Show Gist options
  • Save apetrone/ec4ec30b0012e0b7dc7f to your computer and use it in GitHub Desktop.
Save apetrone/ec4ec30b0012e0b7dc7f to your computer and use it in GitHub Desktop.
roombot
// Adam Petrone, September 2014
// Robot based on original iRobot Roomba
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Adafruit_NeoPixel.h>
// wired up to pins 12, and 13 of Arduino Pro Mini
SoftwareSerial ss = SoftwareSerial(13, 12);
// motor output pins
const uint8_t MOTOR0_FORWARD = 3;
const uint8_t MOTOR0_BACKWARD = 11;
const uint8_t MOTOR1_FORWARD = 9;
const uint8_t MOTOR1_BACKWARD = 10;
const uint8_t LED_DATA_PIN = 4;
const uint8_t TOTAL_PIXELS = 24; // 2 x NeoPixel 12 rings (WS2812) from Adafruit
const int8_t FORWARD = 1;
const int8_t CENTER = 0;
const int8_t BACKWARD = -1;
struct HeadlightControl
{
Adafruit_NeoPixel pixels;
uint8_t enabled;
HeadlightControl() : pixels(TOTAL_PIXELS, LED_DATA_PIN), enabled(0)
{
// the regulator I'm using only delivers about 600mA
// and that's for the LEDs AND logic boards, so let's
// crank this down to half brightness.
pixels.setBrightness(127);
}
void begin()
{
pixels.begin();
}
void toggle()
{
enabled = !enabled;
if (enabled)
{
flood(255, 255, 255);
}
else
{
flood(0, 0, 0);
}
pixels.show();
}
void clear()
{
flood(0, 0, 0);
pixels.show();
}
void flood(uint8_t r, uint8_t g, uint8_t b)
{
for(uint8_t i = 0; i < TOTAL_PIXELS; ++i)
{
pixels.setPixelColor(i, pixels.Color(r, g, b));
}
}
};
struct ButtonState
{
unsigned char state : 4;
// handle a press or release event
void press_release(bool is_down)
{
if ( is_down )
{
// this button was down last update, too
if ( state & 1 )
{
state |= 2;
}
else
{
state |= 1;
state |= 8;
}
}
else
{
// remove 'isDown' flag
state &= ~1;
// set 'released' and 'impulse' flag
state |= 12;
}
}
// update this button state for this frame
void update()
{
// impulse flag
if ( state & 8 )
{
if ( state & 1 ) // button down this update
{
// LOGV( "button %i is down\n", i );
}
else if ( state & 4 ) // button released this update
{
// LOGV( "button %i is released\n", i );
state &= ~1;
}
}
else
{
if ( state & 1 ) // button held
{
state |= 2;
}
else if ( state & 4 ) // button released last update
{
state &= ~4;
state &= ~2;
state &= ~1;
}
}
// clear impulse flag
state &= ~8;
}
}; // ButtonState
struct ControllerInput
{
// [-1, 1] for axis
float axis[2];
// 0/1 to signify button is down
ButtonState buttons[2];
ControllerInput()
{
reset();
}
void update_button(uint8_t index, uint8_t value)
{
buttons[index].press_release(value);
}
void update()
{
buttons[0].update();
buttons[1].update();
}
bool is_down(uint8_t button_id)
{
return (buttons[button_id].state & 1);
}
bool was_released(uint8_t button_id)
{
return (buttons[button_id].state & 2) && !(buttons[button_id].state & 1);
}
void reset()
{
memset(this, 0, sizeof(ControllerInput));
}
};
HeadlightControl headlights;
ControllerInput input;
void setup()
{
// the XBee will use this to save the UART for debugging
ss.begin(57600);
// this will be for debugging
Serial.begin(57600);
// turn off motors.
analogWrite(MOTOR0_FORWARD, 0);
analogWrite(MOTOR0_BACKWARD, 0);
analogWrite(MOTOR1_FORWARD, 0);
analogWrite(MOTOR1_BACKWARD, 0);
headlights.begin();
headlights.clear();
}
float clampf(float input)
{
if (input < -1)
{
input = -1;
}
else if (input > 1)
{
input = 1;
}
return input;
}
void apply_motor_speed(uint8_t forward_pin, uint8_t backward_pin, float normalized_value)
{
int8_t direction = 0;
if (normalized_value > 0)
{
direction = FORWARD;
}
else if (normalized_value < 0)
{
direction = BACKWARD;
normalized_value = -normalized_value;
}
else
{
direction = CENTER;
}
int8_t analog_out = int8_t(255.0 * normalized_value);
if (direction == FORWARD)
{
analogWrite(forward_pin, analog_out);
analogWrite(backward_pin, 0);
}
else if (direction == BACKWARD)
{
analogWrite(forward_pin, 0);
analogWrite(backward_pin, analog_out);
}
else
{
analogWrite(forward_pin, 0);
analogWrite(backward_pin, 0);
}
}
void mix_motor(
uint8_t m0_forward, uint8_t m0_backward,
uint8_t m1_forward, uint8_t m1_backward,
float haxis,
float vaxis)
{
// m0: left
// m1: right
uint8_t m0[2] = {0, 0};
uint8_t m1[2] = {0, 0};
// forward bias
if (vaxis > 0)
{
m0[0] = 255*vaxis;
m1[0] = 255*vaxis;
}
else if (vaxis < 0) // reverse bias
{
m0[1] = 255*(-vaxis);
m1[1] = 255*(-vaxis);
}
if (haxis < 0)
{
if (vaxis > 0)
{
m0[1] = 255 * (vaxis + haxis);
}
else if (vaxis < 0)
{
m0[1] = 255 * (-vaxis + haxis);
}
}
else if (haxis > 0)
{
//m0[0] = 255 * (haxis - vaxis);
}
Serial.print("m0[0] = ");
Serial.print(m0[0]);
Serial.print(" m0[1] = ");
Serial.println(m0[1]);
Serial.print("m1[0] = ");
Serial.print(m1[0]);
Serial.print(" m1[1] = ");
Serial.println(m1[1]);
// analogWrite(m0_forward, m0[0]);
// analogWrite(m0_backward, m0[1]);
//
// analogWrite(m1_forward, m1[0]);
// analogWrite(m1_backward, m1[1]);
}
void loop()
{
if (ss.available() >= 2)
{
int8_t header[2] = {0};
header[0] = ss.read();
header[1] = ss.read();
if (header[0] == 8 && header[1] == 3)
{
int8_t buffer[4] = {0};
buffer[0] = ss.read();
buffer[1] = ss.read();
buffer[2] = ss.read();
buffer[3] = ss.read();
input.axis[0] = clampf(buffer[0] / 117.0f);
input.axis[1] = clampf(buffer[1] / 117.0f);
input.update_button(0, buffer[2]);
input.update_button(1, buffer[3]);
//mix_motor(MOTOR0_FORWARD, MOTOR0_BACKWARD, MOTOR1_FORWARD, MOTOR1_BACKWARD, input.axis[0], input.axis[1]);
}
}
else
{
//Serial.println("awaiting input...");
//input.reset();
input.axis[0] = 0;
input.axis[1] = 0;
}
input.update();
if (input.was_released(0))
{
headlights.toggle();
}
apply_motor_speed(MOTOR0_FORWARD, MOTOR0_BACKWARD, input.axis[0]);
apply_motor_speed(MOTOR1_FORWARD, MOTOR1_BACKWARD, input.axis[1]);
}
// Adam Petrone
// August 31, 2015
// This is an altered version of the original code I wrote
// which adds spektrum receiver compatibility.
// This was designed to work on the Arduino Pro Mini 5v
// It has been tested with a DX6i and these receivers:
// OrangeRX (6-channel)
// AR7000 (7-channel)
// AR6200 (6-channel)
//
// Drop Sensors
// These are open switches while the robot is on the ground.
// When a 'drop' is measured; these close.
// green: rear wheel sensor
// yellow: left wheel sensor
// red: right wheel sensor
//
// Motors + encoders
// The two lower gauge wires go directly to the DC motor.
// As far as the encoders go, fortunately, I found a nice blog post documenting the
// colors for the encoders.
// http://skiptree.com/salvaging-a-roomba/
// blue: IR anode (+)
// orange/brown: IR cathode (-)
// black: emitter
// gray: detector
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Adafruit_NeoPixel.h>
#include <Arduino.h>
#include "spektrum_receiver.h"
//
// CONSTANTS
// output pins for the motor controller
const uint8_t MOTOR0_FORWARD = 3;
const uint8_t MOTOR0_BACKWARD = 11;
const uint8_t MOTOR1_FORWARD = 9;
const uint8_t MOTOR1_BACKWARD = 10;
const uint8_t LED_DATA_PIN = 4;
const uint8_t TOTAL_PIXELS = 24; // 2 x NeoPixel 12 rings (WS2812) from Adafruit
const int8_t FORWARD = 1;
const int8_t CENTER = 0;
const int8_t BACKWARD = -1;
const uint8_t UPDATE_DELAY_MILLISECONDS = 20;
struct HeadlightControl
{
Adafruit_NeoPixel pixels;
HeadlightControl() : pixels(TOTAL_PIXELS, LED_DATA_PIN)
{
// the regulator I'm using only delivers about 600mA
// and that's for the LEDs AND logic boards, so let's
// crank this down to quarter brightness.
set_brightness(63);
}
void begin()
{
pixels.begin();
}
void enable()
{
flood(255, 255, 255);
pixels.show();
}
void disable()
{
flood(0, 0, 0);
pixels.show();
}
void set_brightness(uint8_t brightness)
{
pixels.setBrightness(brightness);
}
void flood(uint8_t r, uint8_t g, uint8_t b)
{
for(uint8_t i = 0; i < TOTAL_PIXELS; ++i)
{
pixels.setPixelColor(i, pixels.Color(r, g, b));
}
}
};
HeadlightControl headlights;
// spektrum receiver, supporting 6 channels
SpektrumReceiver<6> sr;
//
// CODE
// We need this if we're going to read/generate PWM signals
void reset_pwm_timers()
{
// set timer1 to default value
TCCR1B = TCCR1B & (0b11111000 | 0x03);
// set timer2 to default value
TCCR2B = TCCR2B & (0b11111000 | 0x04);
}
// this was designed to be used with a pololu motor driver
// which has separate pins for forward/backward on each h-bridge.
// It accepts a signed 16-bit value for motor speed and direction.
void apply_motor_speed(uint8_t forward_pin, uint8_t backward_pin, int16_t motor_speed)
{
uint8_t analog_speed;
if (motor_speed > 0)
{
analog_speed = map(motor_speed, 0, SHRT_MAX, 0, 255);
analogWrite(forward_pin, analog_speed);
analogWrite(backward_pin, 0);
}
else if (motor_speed < 0)
{
analog_speed = map(-motor_speed, 0, SHRT_MAX, 0, 255);
analogWrite(forward_pin, 0);
analogWrite(backward_pin, analog_speed);
}
else
{
analogWrite(forward_pin, 0);
analogWrite(backward_pin, 0);
}
}
void test_packet()
{
//aux1.update();
// spektrum_packet_t packet;
// packet.channel[0] = 0;
// packet.channel[1] = 1;
// packet.channel[2] = 2;
// packet.channel[3] = 3;
// packet.channel[4] = 4;
// packet.channel[5] = aux1.value();
//
// Serial.write((const char*)&packet, sizeof(spektrum_packet_t));
// Serial.flush();
}
void setup()
{
//reset_pwm_timers();
// turn off motors.
analogWrite(MOTOR0_FORWARD, 0);
analogWrite(MOTOR0_BACKWARD, 0);
analogWrite(MOTOR1_FORWARD, 0);
analogWrite(MOTOR1_BACKWARD, 0);
headlights.begin();
headlights.disable();
// rudder <- positive
// aileron <- positive
// throttle ^ positive
// elevation ^ positive
// You may need to adjust the flaps to ensure the Elevation
// returns the correct travel. I set the elevation to zero for both norm/land flaps.
// which means it isn't modified.
// You can set an axis flip multiplier to ensure the correct directions
// on each of the sticks.
sr.set_channel_pin(SPEKTRUM_CHANNEL_THROTTLE, 13, SPEKTRUM_CHANNEL_STICK, 1);
sr.set_channel_pin(SPEKTRUM_CHANNEL_AILERON, 12, SPEKTRUM_CHANNEL_STICK, -1);
sr.set_channel_pin(SPEKTRUM_CHANNEL_ELEVATION, 8, SPEKTRUM_CHANNEL_STICK, 1);
sr.set_channel_pin(SPEKTRUM_CHANNEL_RUDDER, 7, SPEKTRUM_CHANNEL_STICK, -1);
sr.set_channel_pin(SPEKTRUM_CHANNEL_GEAR, 5, SPEKTRUM_CHANNEL_TOGGLE);
sr.set_channel_pin(SPEKTRUM_CHANNEL_AUX1, 6, SPEKTRUM_CHANNEL_TOGGLE);
// Serial.begin(9600);
}
void headlight_check()
{
int value = sr.value(SPEKTRUM_CHANNEL_GEAR);
if (value > 1850)
{
headlights.set_brightness(31);
headlights.enable();
}
else
{
headlights.disable();
}
}
void loop()
{
sr.update();
// int test = sr.value(SPEKTRUM_CHANNEL_AUX1);
// if (test > 1850)
// {
// Serial.print("throttle: ");
// Serial.println(sr.value(SPEKTRUM_CHANNEL_THROTTLE));
// Serial.print("aileron: ");
// Serial.println(sr.value(SPEKTRUM_CHANNEL_AILERON));
// Serial.print("elevation: ");
// Serial.println(sr.value(SPEKTRUM_CHANNEL_ELEVATION));
// Serial.print("rudder: ");
// Serial.println(sr.value(SPEKTRUM_CHANNEL_RUDDER));
// Serial.print("gear: ");
// Serial.println(sr.value(SPEKTRUM_CHANNEL_GEAR));
// Serial.print("aux1: ");
// Serial.println(sr.value(SPEKTRUM_CHANNEL_AUX1));
// }
headlight_check();
apply_motor_speed(MOTOR0_FORWARD, MOTOR0_BACKWARD, sr.value(SPEKTRUM_CHANNEL_THROTTLE));
apply_motor_speed(MOTOR1_FORWARD, MOTOR1_BACKWARD, sr.value(SPEKTRUM_CHANNEL_ELEVATION));
delay(UPDATE_DELAY_MILLISECONDS);
}
#include "spektrum_receiver.h"
spektrum_packet_t::spektrum_packet_t()
{
memset(this, 0, sizeof(spektrum_packet_t));
header = SPEKTRUM_HEADER_VALUE;
footer = SPEKTRUM_FOOTER_VALUE;
}
bool spektrum_packet_t::is_valid() const
{
return (header == SPEKTRUM_HEADER_VALUE) && (footer == SPEKTRUM_FOOTER_VALUE);
}
// SpektrumChannel::SpektrumChannel(uint16_t pin)
// : data_pin(pin)
// {
// pinMode(data_pin, INPUT);
// }
// void SpektrumChannel::update()
// {
// }
// uint8_t SpektrumChannel::is_value_high() const
// {
// int pulse = pulseIn(data_pin, HIGH);
// return (pulse > 1900);
// }
// uint16_t SpektrumChannel::value() const
// {
// return pulseIn(data_pin, HIGH);
// }
// void SpektrumChannel::setup_motor_channel( uint8_t _forward_pin, uint8_t _backward_pin, uint16_t _min, uint16_t _max, uint16_t _center_min, uint16_t _center_max )
// {
// min_threshold = _min;
// max_threshold = _max;
// center_min = _center_min;
// center_max = _center_max;
// forward_pin = _forward_pin;
// backward_pin = _backward_pin;
// upper_divisor = (max_threshold - center_max);
// lower_divisor = (center_min - min_threshold);
// // output pins for motor driver; normalized output [0, 1]
// pinMode(forward_pin, OUTPUT);
// pinMode(backward_pin, OUTPUT);
// }
// float SpektrumChannel::poll( int8_t & direction )
// {
// int pulse = value();
// int dt = 0;
// float normalizedValue = 0.0;
// //Serial.println(pulse);
// direction = -1;
// if ( pulse > 0 )
// {
// if ( pulse > center_max )
// {
// direction = 1;
// dt = pulse - center_max;
// normalizedValue = ((float)dt/(float)upper_divisor);
// if ( normalizedValue > 1.0 )
// {
// normalizedValue = 1.0;
// }
// }
// else if ( pulse < center_min )
// {
// direction = 0;
// dt = center_min - pulse;
// normalizedValue = ((float)dt/(float)lower_divisor);
// if ( normalizedValue > 1.0 )
// {
// normalizedValue = 1.0;
// }
// }
// else
// {
// normalizedValue = 0.0;
// direction = -1;
// }
// }
// // Serial.print( "Value: " );
// // Serial.println( normalizedValue );
// return normalizedValue;
// }
// void SpektrumChannel::apply_motor_speed()
// {
// int8_t direction = 0;
// int8_t analog_out = 0;
// float normalized_value = 0.0f;
// normalized_value = poll( direction );
// analog_out = int8_t(255.0 * normalized_value);
// if ( direction == 1 )
// {
// analogWrite( forward_pin, analog_out );
// analogWrite( backward_pin, 0 );
// }
// else if ( direction == 0 )
// {
// analogWrite( forward_pin, 0 );
// analogWrite( backward_pin, analog_out );
// }
// else
// {
// analogWrite( forward_pin, 0 );
// analogWrite( backward_pin, 0 );
// }
// }
#pragma once
#include <Arduino.h>
#include <limits.h> // for SHRT_MAX
const uint16_t SPEKTRUM_HEADER_VALUE = 0xbead;
const uint16_t SPEKTRUM_FOOTER_VALUE = 0xfeff;
struct spektrum_packet_t
{
uint16_t header;
int16_t channel[6];
uint16_t footer;
spektrum_packet_t();
bool is_valid() const;
};
// struct SpektrumChannel
// {
// uint16_t min_threshold;
// uint16_t max_threshold;
// uint16_t center_min;
// uint16_t center_max;
// uint16_t upper_divisor;
// uint16_t lower_divisor;
// // input pin from the receiver
// uint16_t data_pin;
// uint8_t forward_pin;
// uint8_t backward_pin;
// SpektrumChannel(uint16_t pin);
// void update();
// uint8_t is_value_high() const;
// uint16_t value() const;
// void setup_motor_channel(uint8_t _forward_pin, uint8_t _backward_pin, uint16_t _min, uint16_t _max, uint16_t _center_min, uint16_t _center_max);
// float poll(int8_t& direction);
// void apply_motor_speed();
// };
enum SpektrumChannelType
{
SPEKTRUM_CHANNEL_STICK, // stick with analog input
SPEKTRUM_CHANNEL_TOGGLE // toggle button (on/off)
};
enum SpektrumChannel
{
SPEKTRUM_CHANNEL_THROTTLE,
SPEKTRUM_CHANNEL_AILERON,
SPEKTRUM_CHANNEL_ELEVATION,
SPEKTRUM_CHANNEL_RUDDER,
SPEKTRUM_CHANNEL_GEAR,
SPEKTRUM_CHANNEL_AUX1,
SPEKTRUM_CHANNEL_AUX2
};
const int32_t MIN_THRESHOLD = 1108;
const int32_t MAX_THRESHOLD = 1875;
const int32_t MIN_CENTER_THRESHOLD = 1490;
const int32_t MAX_CENTER_THRESHOLD = 1515;
const int32_t UPPER_DIV = (MAX_THRESHOLD - MAX_CENTER_THRESHOLD);
const int32_t LOWER_DIV = (MIN_CENTER_THRESHOLD - MIN_THRESHOLD);
static inline int32_t spektrum_update_stick(uint8_t pin)
{
int32_t pulse = pulseIn(pin, HIGH);
int32_t dt;
int32_t value = 0;
if (pulse > 0)
{
// see if the stick is out of the dead-zone
if (pulse > MAX_CENTER_THRESHOLD)
{
dt = pulse - MAX_CENTER_THRESHOLD;
value = ((float)dt/(float)UPPER_DIV) * SHRT_MAX;
}
else if (pulse < MIN_CENTER_THRESHOLD)
{
dt = MIN_CENTER_THRESHOLD - pulse;
value = -((float)dt/(float)LOWER_DIV) * SHRT_MAX;
}
else
{
value = 0;
}
value = constrain(value, -SHRT_MAX, SHRT_MAX);
}
return value;
}
static inline int32_t spektrum_update_toggle(uint8_t pin)
{
return pulseIn(pin, HIGH);
}
template <uint8_t C>
struct SpektrumReceiver
{
uint8_t pins[C];
int32_t values[C];
int8_t axisflip[C];
typedef int32_t (*channel_update_function)(uint8_t pin);
channel_update_function update_channel[C];
SpektrumReceiver()
{
memset(pins, 0, sizeof(uint8_t) * C);
}
void set_channel_pin(uint8_t channel, uint8_t pin, SpektrumChannelType type, int8_t flip = 1)
{
channel_update_function functions[] = {
spektrum_update_stick,
spektrum_update_toggle
};
pins[channel] = pin;
update_channel[channel] = functions[type];
axisflip[channel] = flip;
pinMode(pin, INPUT);
}
void update()
{
for (uint8_t index = 0; index < C; ++index)
{
// don't wait on the serial tx/rx pins (0/1)
if (pins[index] > 1)
{
values[index] = update_channel[index]( pins[index] ) * axisflip[index];
}
}
}
int32_t value(uint8_t channel) const
{
return values[channel];
}
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment