2013Oct11-robotarm

#include <Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;
float theta1;  // target angles as determined through IK
float theta2;
int theta1prev; //old targets for error correction
int theta2prev;
int buf;
int pot1 = 0;
int pot2 = 1;
unsigned long time; //time is really long!
int writingposition = 140;
int idleposition = 170;

void setup() {
  Serial.begin(9600);
  servo1.attach(2,750,2200);
  servo2.attach(3,750,2200);
  //servo3.attach(2,500,2400);
  servo1.write(90);
  servo2.write(90);
  //servo3.write(idleposition);
  pinMode(13,OUTPUT);
}
  
void loop() {
  theta1 = analogRead(pot1);
  theta2 = analogRead(pot2);
  // Serial.print(theta1); Serial.print(","); Serial.println(theta2);
  theta1 = -(theta1/1023)*220+200;
  theta2 = -(theta2/1023)*220+200;
  if(theta1 < 0) theta1 = 0;
  if(theta2 < 0) theta2 = 0;
  if(theta1 > 180) theta1 = 180;
  if(theta2 > 180) theta2 = 180;
  drive_joints();
}

/* drive_joints() simply moves servos 1 and 2 */
void drive_joints() {
  servo1.write(theta1);
  servo2.write(theta2);
  delay(30);
}