Robot arm (2 servo) 2015, python, arduino, processing

arm.py

from math import *
import numpy as np
import matplotlib.pyplot as plt
import parse

l1 = 107
l2 = 110
elbowup = False

def get_angles(Px, Py):
  # first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
  c2 = (pow(Px,2) + pow(Py,2) - pow(l1,2) - pow(l2,2))/(2*l1*l2) # is btwn -1 and 1
  #Serial.print("c2 "); Serial.print(c2);

  if elbowup == False :
    s2 = sqrt(1 - pow(c2,2))  # sqrt can be + or -, and each corresponds to a different orientation

  elif elbowup == True:
    s2 = -sqrt(1 - pow(c2,2))

  theta2 = degrees(atan2(s2,c2))  # solves for the angle in degrees and places in correct quadrant
#theta2 = map(theta2, 0,180,180,0); // the servo is flipped. 0 deg is on the left physically
#now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
  theta1 = degrees(atan2(-l2*s2*Px + (l1 + l2*c2)*Py, (l1 + l2*c2)*Px + l2*s2*Py))
  theta1 = theta1
  return theta1 , theta2


def angles2xy(t1, t2):
    t1 = radians(t1)
    t2 = radians(t2)
    x = cos(t1) * l1 + cos(t1 + t2) * l2
    y = sin(t1) * l1 + sin(t1 + t2) * l2
    return (x, y)

def plotworkingenvelope():
    gridt1, gridt2 = tuple(np.indices([180, 180]))
    gridt1 = gridt1.flatten()
    gridt2 = gridt2.flatten()
    pts = zip(gridt1, gridt2)
    xy = [angles2xy(*pt) for pt in pts]
    plt.scatter([x[0] for x in xy], [x[1] for x in xy], marker="." )

def circle(xcenter, ycenter):
    radius = 10;
    xvals, yvals = [], []
    #xvals, yvals = [-91.59],[5.40]
    for i in np.arange(0,2*pi,0.1):
        xval = xcenter + (sin(i) * radius)
        yval = ycenter + (cos(i) * radius)
        xvals.append(xval)
        yvals.append(yval)
        print "x: " + str(xval) + ", y: " + str(yval)
    return xvals, yvals

#takes x,y values, plots them
# also x,y -> angles -> xy & plots 
def plotIKcircle(temp): 
    xvals, yvals = temp
    plt.plot(xvals, yvals, 'rd') #blue squares
    thetavals = [get_angles(tempx, tempy) for tempx, tempy in zip(xvals, yvals)]
    for i in range(len(thetavals)):
            print thetavals[i]
    IKvals = [angles2xy(theta1, theta2) for theta1, theta2 in thetavals] 
    tempx2, tempy2 = zip(*IKvals)
    plt.axis('equal')
    plt.plot(tempx2, tempy2, 'b.')
    #print "x: " + str(tempx2) + ", y: " + str(tempy2)

def plot_theta_vals(thetavals):
    IKvals = [angles2xy(theta1, theta2) for theta1, theta2 in thetavals] 
    tempx2, tempy2 = zip(*IKvals)
    plt.axis('equal')
    plt.plot(tempx2, tempy2, 'ro')

thetavals = parse.parse_csv()
plot_theta_vals(thetavals)

plotworkingenvelope()
#plotIKcircle(circle(-100,0))
plt.show()

coordinate.pde

class Coordinate {
    private final static int l1 = 113;
    private final static int l2 = 110;

    private float x;
    private float y;
    private float theta1;
    private float theta2;
    private String serialLine;
    
    public Coordinate() {
        this.x = 0;
        this.y = 0;
        this.theta1 = 0;
        this.theta2 = 0;
        this.serialLine = "hello world!";
    }
    
    /* angles2String() takes the angles the servos need
       to go to and puts them in a string
       theta1,theta2'\n' assigned to .serialLine */
    void angles2String(float theta1, float theta2) {
        int angle1 = int(theta1);
        int angle2 = int(theta2);
        //int angle1 = touSec(theta1); //change the angles to microseconds
        //int angle2 = touSec(theta2);
        // these two lines just switch min and max uSecs if the servos are switched
        // angle1 = -angle1 + 2900;
        // angle2 = -angle2 + 2900;
        this.serialLine = angle1 + "," + angle2 + "\n";
    }

    /*Given a point to go to (Px,Py) getAngles()
      solves for the angles the servos need to go
      to and assigns them to .theta1 and .theta2
      It's just inverse kinematics. */
    void getAngles(float Px, float Py) {
        // first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
        float c2 = (pow(Px,2) + pow(Py,2) - pow(l1,2) - pow(l2,2))/(2*l1*l2); //is btwn -1 and 1
        float s2 = sqrt(1 - pow(c2,2));
        this.theta2 = degrees(atan2(s2,c2));  // solves for the angle in degrees and places in correct quadrant
        this.theta1 = degrees(atan2(-l2*s2*Px + (l1 +  l2*c2)*Py,
                                    (l1 + l2*c2)*Px + l2*s2*Py));
    }

    /* touSec() takes an angle in degrees (0-180) and
       converts it into the equivalent microsecond
       value for a servo */
    int touSec(float angle) {
        //angle = (-1)*angle + 180;
        float uSec = (angle/180)*1450 + 750;
        if(uSec - int(uSec) < 0.5) {
            return int(uSec);
        } else {
            return (int(uSec)+1);
        }
    }

    /* mapCoordinates() takes the x and y coordinates
       from the mouse and changes them to numbers
       suitable for the arduino to control the servos*/
    void mapCoordinates(float x, float y, int width, int height) {
        float aspectRatio = (float)(width)/((float)(height));
        if(aspectRatio > 1) {
            this.x = (x/width)*75-150;
            this.y = (-1*((y/height)*75)*(1/aspectRatio))+30;
        } else {
            this.x = (((x/width)*75))*(aspectRatio)-150;
            this.y = (-1*((y/height)*75)+30);
        }

    }
}

lines.txt

117,89

117,89

117,89

paint.pde

import processing.serial.*; //This allows us to use serial objects
 
Serial port; // Create object from Serial class
Coordinate point = new Coordinate();
int width = 600;
int height = 600;

void setup() {
  size(width, height);
  noSmooth();
  fill(126);
  background(102);
  //  println(Serial.list()); //This shows the various serial port option0
  String portName = Serial.list()[0]; //The serial port should match the one the Arduino is hooked to
  port = new Serial(this, portName, 9600); //Establish the connection rate
}

void draw() {}

/* mousePressed() keeps track of mouse clicks
   and sends the coordinates over serial */
void mousePressed() {
  stroke(255);
  ellipse(mouseX,mouseY,5,5);
  if(inbounds(mouseX, mouseY)) {
      point.mapCoordinates(mouseX, mouseY, width, height); //get adjusted coordinates
      point.getAngles(point.x, point.y); //change coordinates to angles
      point.angles2String(point.theta1, point.theta2); //prepare angles for serail
      port.write(point.serialLine); //send the angles over serial
      print(point.serialLine);
  }
}

/* mouseDragged() keeps track of the mouse
   coordinates while being dragged and sends
   them over serial*/
void mouseDragged() {
  stroke(255);
  ellipse(mouseX,mouseY,5,5);
  if(inbounds(mouseX, mouseY)) {
      point.mapCoordinates(mouseX, mouseY, width, height); //get adjusted coordinates
      point.getAngles(point.x, point.y); //change coordinates to angles
      point.angles2String(point.theta1, point.theta2); //prepare them for serial
      port.write(point.serialLine); //send them over serial
      print(point.serialLine);
  }
}

void keyPressed() {
    if(key == 'r') {
        background(102);
    }
}

/* inbounds() checks to see if the mouse position
   is actually on the drawing window */
boolean inbounds(int x, int y) {
    if(x > 0 && y > 0 && x < width && y < height){
        return true;
    } else {
        return false;
    }
}

parse.py

import csv
import sys

def parse_csv():
    f = open("lines.txt")
    try:
        # reader = csv.reader(f)
        # for row in reader:
            # print row
        # input: theta1, theta2
        # output: [[theta1, theta2], [theta1, theta2], ...]
    
        #thetavals = [(map(int,vals)) for vals in csv.reader(f, delimiter=",")] #this accepts empty lines
        thetavals = []
        for row in csv.reader(f, delimiter=","):
            if row: #reject empty lines
                thetavals.append(map(int,row))
    finally:
        f.close()
    return thetavals

serialdraw.ino

// code derived from http://www.instructables.com/id/Robotic-Arm-with-Servo-Motors/?ALLSTEPS
// code derived from http://www.instructables.com/id/Intro-and-what-youll-need/?ALLSTEPS
// derived from http://arduino.cc/en/Tutorial/Graph
// code derived from http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino
//l1 67.5 mm
//l2 65.8mm
// this code relies on processing to do the inverse kinematics and send over theta values (0 to 180. use writeMicroseconds if need more detail) 

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

void setup() {
  Serial.begin(9600);
  servo1.attach(2,544,2400);
  servo2.attach(3,544,2400);
  servo3.attach(4,544,2400);
  servo1.write(90);
  servo2.write(80);
  servo3.write(idleposition);
  pinMode(13,OUTPUT);
}
  
void loop() {
  //if(Serial.available() > 0) { //check for data
  if (Serial.available() > 0) {
    readAngles(); //get the angles
    if(theta1 > 180 || theta1 < 0) { //these two if statements check if the data is reasonable.  When using drag in processing sometimes absurd numbers get sent
      theta1 = theta1prev;
    }
    if(theta2 > 180 || theta2 < 0) {
      theta2 = theta2prev;
    }
    drive_joints();
    if(!writing()){
      startWritingNow();
    }
    theta1prev = theta1;
    theta2prev = theta2;
    time = millis();
    } else if(millis() - time > 300 && writing) { //move the pen(servo3) up when done
    stopWritingNow();
  }
}

/* drive_joints() simply moves servos 1 and 2 */
void drive_joints() {
 // theta1 = map(theta1, 1000,2000,2000,1000);
 // theta2 = map(theta2, 1000,2000,2000,1000);
  theta1 = map(theta1, 0, 180, 180, 0); 
  theta2 = map(theta2, 0, 180, 180, 0);
  servo1.write(theta1);
  servo2.write(theta2);
}

/* readAngles() reads the doubles from the serial port
   it is inteded to take in data of the form 2.2,3.4'\n'
   where there is only one decimal place and the values
   are sperated by a comma and a newline after the tuple*/   
void readAngles() {
  theta1 = Serial.parseInt();
  theta2 = Serial.parseInt();
  if (Serial.read() == '\n') {
    //Serial.print("t1: ");
    //Serial.print(theta1);
    //Serial.print(", t2: ");
    //Serial.println(theta2);
  }
  }
  
//  byte character = Serial.read(); //read the first byte on serial
//  while(character != 10) {
//    if(character != ','){ //newline(10) and comma are special
//      buf = buf*10;
//      buf += (int)(character - '0'); //these two lines turn the string into an integer by subtracting ascii(0)=48 from it
//    } else if(character == ',') {
//      theta1 = buf; //after a comma the buffer has the first angle
//      Serial.print("theta1: ");
//      Serial.print(theta1);
//      buf = 0;
//    } else {
//      //      theta2 = buf; //after a newline the buffer has the second angle
//      buf = 0;
//    }
//    while(Serial.available() == 0) {
//      delayMicroseconds(100);
//    }
//    character = Serial.read();
//  }
//  theta2 = buf;
//  Serial.print(", theta2: ");
//  Serial.println(theta2);
//  buf = 0;

/* startWritingNow() moves servo3Minto writing position
   there is a delay to wait for the other servos to get
   into position before it makes a mark */
void startWritingNow() {
  servo3.write(writingposition);
}

/* stopWritingNow() moves servo3 off the page */
void stopWritingNow() {
  servo3.write(idleposition);
}

boolean writing() {
  if(abs(servo3.read() - writingposition) < 1) {
    return true;
  } else {
    return false;
  }
}