pan_tilt_receiver

pan_tilt_receiver.ino

// Adam Petrone
// October 2014
// Code to control my custom pan/tilt mechanism.
// Target Hardware: Arduino Pro Mini 8 or 16MHz should do.


#include <Servo.h>
#include <SoftwareSerial.h>
// DOUT, DIN on the XBee to pins 6 and 7 respectively on the Pro Mini.
SoftwareSerial ss = SoftwareSerial(6, 7);

const uint8_t PAN_SERVO_PIN = 3;
const uint8_t TILT_SERVO_PIN = 9;

Servo pan, tilt;

// State:
// This mechanism can be in one of two primary states.
// Auto or Manual - based on whether it's automatically panning
// or under manual remote control.

const uint8_t MODE_AUTO     = (1 << 0); // auto pan mode
const uint8_t MODE_MANUAL   = (1 << 1); // manual mode; controlled by remote
const uint8_t HOLD_VALUE    = (1 << 2); // hold current position
const uint8_t PAN_RIGHT     = (1 << 3); // panning right; else panning left
const uint8_t PAN_LEFT      = (1 << 4);
const uint8_t IS_PANNING    = (1 << 5);

uint8_t MIN_PAN = 0;
uint8_t MAX_PAN = 120;

float pan_value = 0;
uint8_t state_flags = 0;
uint8_t values[2] = {0};
int16_t pan_delay = 0;

float clampf(float input)
{
  if (input < -1)
  {
    input = -1;
  }
  else if (input > 1)
  {
    input = 1;
  }
  
  return input;
}

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));    
  }
  
  void receive(SoftwareSerial& ss)
  {
    if (ss.available() >= 4)
    {
        int8_t buffer[4] = {0};
        buffer[0] = ss.read();
        buffer[1] = ss.read();
        buffer[2] = ss.read();
        buffer[3] = ss.read();
  
        axis[0] = clampf(buffer[0] / 117.0f);
        axis[1] = clampf(buffer[1] / 117.0f);
        update_button(0, buffer[2]);
        update_button(1, buffer[3]);
    }
    
    update();    
  }
};


void test_servo(Servo& servo)
{
  servo.write(0);
  delay( 1000 );
  
  servo.write(45);
  delay( 1000 );

  servo.write(90);
  delay( 1000 );  
  
  servo.write(135);
  delay( 1000 );  
  
  servo.write(180);
  delay( 1000 );
}

ControllerInput input;

void setup()
{
  Serial.begin(57600);
  ss.begin(57600);
  
  state_flags = MODE_AUTO | PAN_LEFT | IS_PANNING;
  values[0] = MIN_PAN;
  values[1] = 115;  
  
  pan.attach(PAN_SERVO_PIN);
  tilt.attach(TILT_SERVO_PIN);
  
  // test
  pan.write(values[0]);
  tilt.write(values[1]);
  
  // wait until the servos reset
  delay(15);
}


void loop()
{
  // listen to the remote control; if there's one connected
  // THIS IS DISABLED FOR NOW: 
  // The transmitter is sending bad data once in a while.
  // Until there is time to track it down, I'm leaving this as a slow, automatic pan/tilt gimbal.
  //input.receive(ss);
  
  // if we're not holding values...
  if (!(state_flags & HOLD_VALUE))
  {
    
    // if we're in manual mode
    if (state_flags & MODE_MANUAL)
    {
      // input values will be from [-1, 1]
      // 0 needs to equate to 90 degrees rotation on the servo
      // since that's how I've centered the gimbal.
  
      // invert the X-axis
      if (input.axis[0] <= 0)
      {
        uint8_t v = 255 * -input.axis[0];
        values[0] = map(v, 0, 255, 90, 180);
      }
      else
      {
        uint8_t v = 255 * (1.0-input.axis[0]);
        values[0] = map(v, 0, 255, 0, 90);
      }
      
      if (input.axis[1] <= 0)
      {
        uint8_t v = 255 * -input.axis[1];
        values[1] = map(v, 0, 255, 90, 0);
      }
      else
      {
        uint8_t v = 255 * input.axis[1];
        values[1] = map(v, 0, 255, 90, 180);
      }
    }
    else if (state_flags & MODE_AUTO)
    {
      if (state_flags & IS_PANNING)
      {
        if (state_flags & PAN_RIGHT)
        {
          pan_value -= 0.1;
        }
        else if (state_flags & PAN_LEFT)
        {
          pan_value += 0.1;
        }
        
        if (pan_value > MAX_PAN)
        {
          pan_delay = 1000;
          state_flags &= ~IS_PANNING;
          pan_value = MAX_PAN;
        }
        else if (pan_value <= MIN_PAN)
        {
          pan_delay = 1000;        
          state_flags &= ~IS_PANNING;   
          pan_value = MIN_PAN;
        }
      }
      else
      {
        pan_delay -= 10;
        if (pan_delay <= 0)
        {
          state_flags |= IS_PANNING;
          state_flags ^= PAN_LEFT;
          state_flags ^= PAN_RIGHT;          
        }
      }
      
      values[0] = uint8_t(pan_value);
    }
  }
  
  // toggle hold values
  if (input.was_released(0))
  {
    state_flags ^= HOLD_VALUE;
  }
  
  if (input.was_released(1))
  {
    // switch off between manual/auto
    state_flags ^= MODE_MANUAL;
    state_flags ^= MODE_AUTO;
    
    // reset pan values
    state_flags |= PAN_LEFT | IS_PANNING;
    pan_delay = 0;
    values[0] = 0;
    values[1] = 115;
  }
  
  pan.write(values[0]);
  tilt.write(values[1]);

  delay(10);
}