commit d67c62b5c4bba98ea883c5ecf632c0c8475c4f0f
Author: James Devine <pingud98@users.noreply.github.com>
Date:   Thu Dec 26 15:49:41 2013 -0800

diff --git a/ultimatearm2.pde b/ultimatearm2.pde
new file mode 100644
index 0000000..88ee541
--- /dev/null
+++ b/ultimatearm2.pde
@@ -0,0 +1,205 @@
+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;
+}
+