roombot

roomba_robot.ino

// 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]);
}

roomba_robot2.ino

// 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);
}

spektrum_receiver.cpp

#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 );
// 	}
// }

spektrum_receiver.h

#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];
	}
};