/* built with Studio Sketchpad:
* https://sketchpad.cc
*
* observe the evolution of this sketch:
* https://studio.sketchpad.cc/sp/pad/view/ro.0$Cs8X$VV72/rev.4944
*
* authors:
*
* Scott Mahr
* license (unless otherwise specified):
* creative commons attribution-share alike 3.0 license.
* https://creativecommons.org/licenses/by-sa/3.0/
*/
// Pressing Control-R will render this sketch.
//Things to do
//gcode control
//
string ncFile = "( Made using CamBam - http://www.cambam.co.uk )/n"+
"( Untitled 5/26/2011 2:09:03 PM )/n"+
"( T0 : 0.0 )/n"+
"G21 G90 G64 G40/n"+
"G0 Z3.0/n"+
"( T0 : 0.0 )/n"+
"T0 M6/n"+
"( Profile1 )/n"+
"G17/n"+
"M3 S1000/n"+
"G0 X0.0 Y0.0/n"+
"G1 F300.0 Z0.0/n"+
"G1 F800.0 X6.0/n"+
"G1 Y8.0/n"+
"G1 X3.0/n"+
"G1 X0.0 Y5.0/n"+
"G1 Y0.0/n"+
"G0 Z3.0/n"+
"M5/n"+
"M30/n";
//Stuff for positioning
int[] p1={10,50}; // x1 = 10;int y1 = 50;
int[] p2={290,50}; // int x2 = 290;int y2 = 50;
int sep = p2[0]-p1[0];
motor m1 = new motor()
m1.len=180;
motor m2 = new motor()
m2.len=110;
float a; float b; float h;
bool drawOn = false;
gc currentPos = new gc(0,0,1);
gc targetPos = new gc(0,0,1);
gc nextPos = new gc(0,0,1);
float feed = 2.0; //2 pix per frame
//drawing
int mem = 500;
int[][] pos = new int[mem][2];
int posIdx = 0;
//Arc Stuff
float arcDist = 1;
//This is target positions, plus move type
gc[] cmds = {new gc(186,87),
new gc(50,100),
new gc(90,171.23),
new gc(200,200),};
cmds[2].initArc(50,100,30,30);
int targetIdx = 0;
//Arc Stuff
void setup() { // this is run once.
background(1);// set the background color
size(300, 300); // canvas size (Integers only, please.)
smooth();// smooth edges
frameRate(30);// limit the number of frames per second
calcPosition();
//println("Current "+currentPos.txt());
drawOn = true;
targetPos = cmds[targetIdx];
//println("target "+ targetPos.txt());
//targetPos.calcLengths();
targetIdx++;
}
void draw() { // this is run repeatedly.
//cleanup
smooth();
background(0);
noStroke();
fill(226);
ellipse(80, 130, 1, 2); //p1
drawBackground();
if(checkTarget()){
if(targetIdx<cmds.length){
targetPos = cmds[targetIdx];
targetIdx++;
} else{drawOn=false;}
} else{
move();
}
calcPosition();
drawPath();
drawBot();
}
void move(){ //Try moving to a point in a straight line
if(targetPos.G ==1){
float distPct = feed/calcDist(currentPos,targetPos);
nextPos.x = currentPos.x + (targetPos.x-currentPos.x)*distPct;
nextPos.y = currentPos.y + (targetPos.y-currentPos.y)*distPct;
} else if(targetPos.G ==2){
//println("here");
gc c = new gc(targetPos.cx,targetPos.cy);
//println(c.txt());
gc cXaxis = new gc(c.x+100,c.y);
float angleX = calcAngle(c,currentPos,cXaxis);
//println("angle " +angleX);
float sinSign = -(currentPos.y-c.y)/Math.abs((currentPos.y-c.y));
//println(sinSign);
float finalAngle = angleX+sinSign*targetPos.dTheta;
//println("finalAngle " +finalAngle );
nextPos.x = c.x+Math.cos(finalAngle)*targetPos.r;
nextPos.y = c.y-sinSign*Math.sin(finalAngle)*targetPos.r;
//println("currentPos "+currentPos.txt());
//println("nextpos "+nextPos.txt());
//delay(10000);
}
nextPos.calcLengths();
m1.len = nextPos.l1;
m2.len = nextPos.l2;
}
//Checks if we are close to the target
bool checkTarget(){
//println("dist "+ calcDist(currentPos,targetPos));
if(calcDist(currentPos,targetPos) <= 2){return true;}
return false;
}
float calcAngle(gc o, gc a, gc b){
//origin, a, b
gc aV = new gc(a.x-o.x, a.y-o.y);
gc bV = new gc(b.x-o.x, b.y-o.y);
float aD = calcDist(o,a);
float bD = calcDist(o,b);
//println("av "+aV.txt());println("bv "+bV.txt());println("len "+aD+" "+bD);
float angle = Math.acos((aV.x*bV.x+aV.y*bV.y)/(aD*bD));
//return angle/(2*Math.PI)*360;
return angle;
}
float calcDist(gc a, gc b){
return Math.sqrt( Math.pow(a.x-b.x,2) + Math.pow(a.y-b.y,2) );
}
float calcDist2(float x1,float y1,float x2,float y2){
return Math.sqrt( Math.pow(x2-x1,2) + Math.pow(y2-y1,2) );
}
void calcPosition(){
//Finding position from l1 and l2
a = (-(m2.len*m2.len)+(m1.len*m1.len)+(sep*sep))/(2*sep);
b = sep - a;
h = Math.sqrt(m1.len*m1.len-a*a);
currentPos.x = p1[0]+a;
currentPos.y = p1[1]+h;
if(drawOn){
pos[posIdx][0]=currentPos.x;
pos[posIdx][1]=currentPos.y;
posIdx +=1;
if(posIdx >= mem){posIdx = 0;}
}
}
void drawBackground(){
//Draw two anchor points
strokeWeight(2);// set the width of the line.
stroke(250, 250, 250, 100);
fill(250, 250, 250, 100);
ellipseMode(CENTER);
ellipse(p1[0], p1[1], 5, 5);
ellipse(p2[0], p2[1], 5, 5);
for(int i = 0; i < cmds.length; i++){
ellipse(cmds[i].x, cmds[i].y, 5, 5);
}
strokeWeight(1);// set the width of the line.
}
void drawBot(){
stroke(255,255,255)
ellipse(currentPos.x, currentPos.y, 5, 5);
line(p1[0], p1[1], currentPos.x,currentPos.y); // draw the line
line(p2[0], p2[1], currentPos.x,currentPos.y); // draw the line
}
void drawPath(){
for(int i = 1; i < mem; i = i+1){
stroke(0,255,0);
point(pos[i][0], pos[i][1]);
}
}
class gc{
float x;
float y;
float l1 = 0;
float l2 = 0;
int G;
//Arc Stuff
float i = 0;
float j = 0;
float r = 0;
float cx = 0;
float cy = 0;
float dTheta = 0;
float curTheta = 0;
gc(float xx,float yy) {
x=xx;
y=yy;
G=1;
}
string txt(){return x+" "+y;}
string txt2(){
return x+" "+y+" "+i+" "+j+" "+cx+" "+cy;
}
bool initArc(float xi,float yi,float ii,float jj){
G=2;
i = ii;
j = jj;
cx = xi+i;
cy = yi+j;
r = calcDist2(xi,yi,cx,cy);
dTheta = arcDist/r;
}
void calcLengths () {
l1 = Math.sqrt( Math.pow(y-p1[1],2) + Math.pow(x-p1[0],2));
l2 = Math.sqrt( Math.pow(y-p2[1],2) + Math.pow(x-p2[0],2));
}
}
class motor{
float len;
float vel;
motor() {}
}
bool testArc(gc a,gc b,float i, float j,int wise){
float sinSign = j/Math.abs(j);
gc c = new gc(a.x+i,a.y+j);
gc cXaxis = new gc(c.x+100,c.y);
float r = calcDist(a,c);
angleDif = arcDist/r;
float angle = calcAngle(c,a,b);
if(Math.abs(angle)<angleDif){return true;}
float angleX = calcAngle(c,a,cXaxis);
float finalAngle = angleX-sinSign*wise*angleDif;
gc final = new gc(c.x+Math.cos(finalAngle)*r,c.y-sinSign*Math.sin(finalAngle)*r);
//println("angle "+angleX);/println("finalangle "+finalAngle);println("final "+final.txt());
//println("c "+c.txt());println("a "+a.txt());println("b "+b.txt());println("angle "+angle);
ellipse(c.x, c.y, 3, 3); //center
ellipse(a.x, a.y, 3, 3); //p1
ellipse(b.x, b.y, 3, 3); //p2
ellipse(final.x, final.y, 3, 3); //p2
line(c.x,c.y,a.x,a.y); // draw the line
line(c.x,c.y,b.x,b.y);
return false;
}