Crappybot

gistfile1.txt

int LEFT = A0;
int RIGHT = A1;

int SERVO_MAX = 180.0;

int x_max;
int y_max;

Servo lt_servo;
Servo rt_servo;

void setup() {
  Serial.begin(9600);

  pinMode(LEFT, OUTPUT);
  pinMode(RIGHT, OUTPUT);
 
  Spark.function("foward", forward);
  Spark.function("back", back);
  Spark.function("left", left);
  Spark.function("right", right);

  lt_servo.attach(LEFT);
  rt_servo.attach(RIGHT);

  x_max = 180.0;
  y_max = 180.0;
}

void loop() {
    Serial.println("")
}

int forward(String args) {
    drive(0, 1);
    return 1;
}

int back(String args) {
    drive(0, -1);
    return 1;
}

int left(String args) {
    drive(-1, 0);
    return 1;
}

int right(String args) {
    drive(1, 0);
    return 1;
}

int stop(String arg) {
    drive(0, 0);
    return 1;
}

int drive(int x, int y) {
  double left;
  double right;

  x = x - (x_max/2);
  y = y - (y_max/2);
  
  left = y + x;
  right = y - x;
  
  left = min((left * SERVO_MAX/x_max) + SERVO_MAX/2, 180);
  right = min((right * SERVO_MAX/y_max) + SERVO_MAX/2, 180);
  
  rt_servo.write(right);
  lt_servo.write(reverse(left));
  
  Serial.print("L: ");
  Serial.print(left);
  Serial.print(", R: ");
  Serial.print(right);
 
  return 1;
}

int reverse(int val) {
  return abs(val - 180);
}