Skip to content

Instantly share code, notes, and snippets.

@nouyang nouyang/arm.py
Last active Feb 22, 2019

Embed
What would you like to do?
Robot arm (2 servo) 2015, python, arduino, processing
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()
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);
}
}
}
117,89
117,89
117,89
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;
}
}
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
// 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;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.