Create a gist now

Instantly share code, notes, and snippets.

transmitting x,y coordinates back and forth between arduino and processing
int buf = 0;
int xcoordinate = 0;
int ycoordinate = 0;
void setup()
{
// start serial port at 9600 bps and wait for port to open:
Serial.begin(115200);
}
void loop()
{
if (Serial.available() > 0) {
readCoordinates();
delay(2); //9600 baud rate is slower than the arduino clock speed this delay removes problems
if(Serial.available() == 0){ //this is where you would move the arm to a coordinate I just print them.
Serial.print(xcoordinate);
Serial.print(",");
Serial.println(ycoordinate);
}
}
}
/* This function reads serial data and interprets it as
integer coordinates. The format should be a string of
two numbers seperated by only a comma with a new line
exactly at the end. */
void readCoordinates(){
byte character = Serial.read(); //read the first byte on serial
if(character != 10 && character != ','){ //newline(10) and , are special
buf = buf*10;
buf += (int)(character - '0'); //these two lines turn the string into an integer
} else if(character == ','){
xcoordinate = buf; //after a comma the buffer has the x coordinate
buf = 0;
} else {
ycoordinate = buf; //after a space the buffer has the y coordinate
buf = 0;
}
}
/**
* Mouse Press.
*
* Move the mouse to position the shape.
* Press the mouse button to invert the color.
*/
import processing.serial.*; //This allows us to use serial objects
Serial port; // Create object from Serial class
int[] x = new int[0];
int[] y = new int[0];
int[] shoulder = new int[0];
int[] elbow = new int[0];
// int[] xylines = new int[0];
float xcoordinate;
float ycoordinate;
String xyline;
float theta1;
float theta2;
boolean elbowup = false; // true=elbow up, false=elbow down
float c2; //is btwn -1 and 1
float s2;
float l1 = 74; //length of links in mm
float l2 = 53;
int width = 700;
int height = 700;
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)) {
mapCoordinates(mouseX, mouseY); //get adjusted coordinates
getAngles(xcoordinate, ycoordinate); //change coordinates to angles
sendAngles(); //send the angles over serial
shoulder = append(shoulder, int(theta1));
elbow = append(elbow, int(theta2));
x = append(x, int(xcoordinate));
y = append(y, int(ycoordinate));
}
}
/* 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)) {
mapCoordinates(mouseX, mouseY); //get adjusted coordinates
getAngles(xcoordinate, ycoordinate); //change coordinates to angles
sendAngles(); //send them over serial
shoulder = append(shoulder, int(theta1));
elbow = append(elbow, int(theta2));
x = append(x, int(xcoordinate));
y = append(y, int(ycoordinate));
}
}
/* sendAngles() sends two ints over serial in
the form 2,3'\n' for the arudino to parse*/
void sendAngles() {
int angle1 = touSec(theta1);
int angle2 = touSec(theta2);
xyline = angle1 + "," + angle2 + "\n";
String lol = int(xcoordinate) + "," + int(ycoordinate);
print("Sent: " + lol + ',' + xyline);
port.write(xyline);
}
//Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void getAngles(float Px, float 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
//if (elbowup == false) { # for now, commented out because assume always elbowdown solution
s2 = sqrt(1 - pow(c2,2)); //sqrt can be + or -, and each corresponds to a different orientation
theta2 = degrees(atan2(s2,c2)); // solves for the angle in degrees and places in correct quadrant
theta1 = degrees(atan2(-l2*s2*Px + (l1 + l2*c2)*Py,
(l1 + l2*c2)*Px + l2*s2*Py));
}
/* 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;
}
}
/* mapCoordinates() takes the x and y cooridantes
from the mouse and changes them to numbers
suitable for the arduino to control the servos*/
void mapCoordinates(float x, float y) {
xcoordinate = -1*(x/width)*50;
ycoordinate = -1*(y/height)*50+90;
}
void keyPressed() { // Press a key to save the data
String[] lines = new String[x.length];
for (int i = 0; i < x.length; i++) {
lines[i] = int(touSec(shoulder[i])) + "," + int(touSec(elbow[i])) + "," + x[i] + "," + y[i];
}
saveStrings("/home/cappie/sketchbook/lines.txt", lines);
exit(); // Stop the program
}
int touSec(float angle) {
return int((angle/180)*1900 + 500);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment