Skip to content

Instantly share code, notes, and snippets.

@pingud98
Created December 26, 2013 23:49
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save pingud98/8140322 to your computer and use it in GitHub Desktop.
Save pingud98/8140322 to your computer and use it in GitHub Desktop.
Processing code for my robot arm as demonstrated at Lift 11 NB the code in the arduino is just Firmata exposing the three servos
import processing.serial.*;
import cc.arduino.*;
Arduino arduino;
int countstart = 0;
int servo1Pin = 9; // Control pin for servo motor
int servo2Pin = 10; // Control pin for servo motor
int servo3Pin = 11; // Control pin for servo motor
int armposn;// = 0;
int digitposn;// = 0;
//int penposn;//= 0;
int penstate = 0;
int xcent = 300;
int ycent = 300;
//int targetX= 400;
//int targetY= 200;
int line1 = 130;
int line2 = 130;
int minimumlength = 40;
color black = color(0);
PImage img;
PImage edgeImg ;
void setup(){
float[][] kernel = { { -1, -1, -1 },
{ -1, 9, -1 },
{ -1, -1, -1 } };
size (600, 600);
background(255);
img = loadImage("james.JPG"); // Load the original image
img.loadPixels();
edgeImg = createImage(img.width, img.height, RGB);
// Loop through every pixel in the image.
for (int y = 1; y < img.height-1; y++) { // Skip top and bottom edges
for (int x = 1; x < img.width-1; x++) { // Skip left and right edges
float sum = 0; // Kernel sum for this pixel
for (int ky = -1; ky <= 1; ky++) {
for (int kx = -1; kx <= 1; kx++) {
// Calculate the adjacent pixel for this kernel point
int pos = (y + ky)*img.width + (x + kx);
// Image is grayscale, red/green/blue are identical
float val = red(img.pixels[pos]);
// Multiply adjacent pixels based on the kernel values
sum += kernel[ky+1][kx+1] * val;
}
}
// For this pixel in the new image, set the gray value
// based on the sum from the kernel
edgeImg.pixels[y*img.width + x] = color(sum);
}
}
// State that there are changes to edgeImg.pixels[]
edgeImg.updatePixels();
arduino = new Arduino(this, Arduino.list()[0]);
arduino.pinMode(servo1Pin, Arduino.OUTPUT);
arduino.pinMode(servo2Pin, Arduino.OUTPUT);
arduino.pinMode(servo3Pin, Arduino.OUTPUT);
arduino.analogWrite(servo1Pin, 70);
arduino.analogWrite(servo2Pin, 170);
arduino.analogWrite(servo3Pin, 50); // the servo moves to the horizontal location of the mouse
// note - we are setting a digital pin to output
background(255);
}
void draw()
{
if (countstart == 1){
makepic();
arduino.analogWrite(servo1Pin, 70);
arduino.analogWrite(servo2Pin, 170);
arduino.analogWrite(servo3Pin, 50);
}
countstart = countstart +1;
}
//void draw(){
void makepic(){
background(255);
//fill(50);
stroke(0);
//rect(350+30,320-10,130,130);
image(img, 0, 0, 130, 130); // Displays the image from point (0,0)
image(edgeImg, 130, 0, 130, 130); // Draw the new image
filter(POSTERIZE,4);
filter(THRESHOLD);
colorMode(HSB);
for (int xcycle = 130; xcycle < 259; xcycle = xcycle+2)
{
for (int ycycle = 0; ycycle < 129; ycycle++)
{
if (get(xcycle,ycycle)<-2)
{
set(xcycle+350-130+30,ycycle+320-10,black);
markpaper(xcycle+350-130+30,ycycle+320-10,140);
//delay(200);
}
else
{
set(xcycle,ycycle,0);
markpaper(xcycle+350-130+30,ycycle+320-10,170);
//delay(50);
}
}
for (int ycycle = 129; ycycle > 0; ycycle--)
{
if (get(xcycle+1,ycycle)<-2)
{
set(xcycle+350-130+1+30,ycycle+320-10,black);
markpaper(xcycle+350-130+1+30,ycycle+320-10,140);
//delay(200);
}
else
{
set(xcycle+1,ycycle,0);
markpaper(xcycle+350-130+1+30,ycycle+320-10,170);
//delay(50);
}
}
}
}
void markpaper(int targetX, int targetY, int penposn){
//print("xposn" + targetX);
//println(" yposn" + targetY);
float angle1 = atan2((targetX - xcent), (targetY - ycent));
float sectorlength = sqrt(sq(targetX - xcent)+sq(targetY - ycent));
//if it's out of reach, shorten the length in the same direction
if (sectorlength > (line1+line2))
{
sectorlength = line1+line2;
targetX = int(xcent + sin(angle1)*sectorlength);
targetY = int(ycent + cos(angle1)*sectorlength);
}
if (sectorlength < minimumlength)
{
sectorlength = minimumlength;
targetX = int(xcent + sin(angle1)*sectorlength);
targetY = int(ycent + cos(angle1)*sectorlength);
}
float internangle = acos((sq(sectorlength)-sq(line2)-sq(line1))/(2*line1*line2));
float sendangle1st = (angle1+(internangle/2));
if (degrees(sendangle1st) < 0 )
{
sendangle1st = radians(0);
}
if (degrees(sendangle1st) > 180 )
{
sendangle1st = radians(180);
}
int line1X = int(xcent+ sin(sendangle1st)*line1);
int line1Y = int(ycent+ cos(sendangle1st)*line1);
int line2X = line1X + int(sin(((angle1+(internangle/2) - internangle)))*line2);
int line2Y = line1Y + int(cos(((angle1+(internangle/2) - internangle)))*line2);
stroke(0,9);
line(xcent, ycent, line1X, line1Y);
line(line1X, line1Y, line2X-1, line2Y-1);
// if (penposn == 150)
// {
// set(line2X,line2Y,black);
// println("TEXT");
// }
// set(mouseX,mouseY,black);
//if it's too close set the minimum threshold in the same direction
int sendangle1 = round(degrees(sendangle1st));
int sendangle2 = round(degrees((radians(180) - internangle)-radians(35)));
//println("Send1>"+ sendangle1 + " Send2 >" + sendangle2);
digitposn = sendangle2;
armposn = sendangle1;
arduino.analogWrite(servo1Pin, digitposn);
arduino.analogWrite(servo2Pin, penposn);
arduino.analogWrite(servo3Pin, armposn);
if (penstate != penposn)
{
delay(300);
}
else
{
delay(1);
}// the servo moves to the horizontal location of the mouse
penstate = penposn;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment