MeArmJoystick

/*
 * VERSION: 0.1
 * LICENSE: GPL v3 (http://www.gnu.org/licenses/gpl.html)
 * AUTHOR: Tiago Queiroz (github.com/belimawr)
 */
#include <Servo.h>


/*
 * You need to find the values that work for you.
 * The Servo Knob example is quite useful to do that.
 */
#define MAX_X 180
#define MAX_Y 180
#define MAX_Z 180
#define MAX_G 120

#define MIN_X 0
#define MIN_Y 0
#define MIN_Z 0
#define MIN_G 90


int basePin = 11;
int shoulderPin = 10;
int elbowPin = 9;
int gripperPin = 8;
Servo elbow, shoulder, base, gripper;

int xdirPin = 0;
int ydirPin = 1;
int zdirPin = 2;
int gdirPin = 3;

/*
 * The default position.
 */
int x = 90;
int y = 90;
int z = 90;
int g = 90;

void setup() {
  /* Debug is always good!*/
  Serial.begin(9600);
  
  elbow.attach(elbowPin);
  shoulder.attach(shoulderPin);
  base.attach(basePin);
  gripper.attach(gripperPin);
  
  elbow.write(x);
  shoulder.write(y);
  base.write(z);
  gripper.write(g);
}

void loop() {
  /*
   * The idea is to detect the direction of the movement,
   * you can also use buttons attached to digital pins.
   * You might need to fine tune those values to "center"
   * your joystick
   */
  float dx = map(analogRead(xdirPin), 0, 1023, -5, 5);
  float dy = map(analogRead(ydirPin), 0, 1023, -5, 5);
  float dz = map(analogRead(zdirPin), 0, 1023, -5, 5);
  float dg = map(analogRead(gdirPin), 0, 1023, -5, 5);
  
  x += dx;
  y += dy;
  z += dz;
  g += dg;
  
  if(x > MAX_X)
    x = MAX_X;
  if(x < MIN_X)
    x = MIN_X;
    
  if(y > MAX_Y)
    y = MAX_Y;
  if(y < MIN_Y)
    y = MIN_Y;
    
  if(z > MAX_Z)
    z = MAX_Z;
  if(z < MIN_Z)
    z = MIN_Z;
    
  if(g > MAX_G)
    g = MAX_G;
  if(g < MIN_G)
    g = MIN_G;
    
    
  Serial.print("(");
  Serial.print(x);
  
  Serial.print(", ");
  Serial.print(y);
  
  Serial.print(", ");
  Serial.print(z);
  
  Serial.print(") - ");
  Serial.println(g);
  
  elbow.write(x);
  shoulder.write(y);
  base.write(z);
  gripper.write(g);

  /*
   * A wee delay, so it does not move too fast
   */
  delay(100);
}