Skip to content

Instantly share code, notes, and snippets.

@apetrone
Last active August 29, 2015 14:07
Show Gist options
  • Save apetrone/b76ae25269d0e4f810b7 to your computer and use it in GitHub Desktop.
Save apetrone/b76ae25269d0e4f810b7 to your computer and use it in GitHub Desktop.
pan_tilt_receiver
// 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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment