{{ message }}

Instantly share code, notes, and snippets.

# nouyang/arm.py

Last active Feb 22, 2019
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 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; } }