/* built with Studio Sketchpad:
* https://sketchpad.cc
*
* observe the evolution of this sketch:
* https://studio.sketchpad.cc/sp/pad/view/ro.Xt1F0Gjujua/rev.417
*
* authors:
* Sam Kronick
* license (unless otherwise specified):
* creative commons attribution-share alike 3.0 license.
* https://creativecommons.org/licenses/by-sa/3.0/
*/
int GRID_SIZE = 20;
Arm[] arms;
void setup() {
size(800,600);
colorMode(HSB);
smooth();
frameRate(60);
arms = new Arm[4];
arms[0]= new Arm(400, 200, new XY(0,0));
arms[1]= new Arm(400, 200, new XY(width,0));
arms[2]= new Arm(400, 200, new XY(width,height));
arms[3]= new Arm(400, 200, new XY(0,height));
}
void draw() {
background(140,50,180);
for(int i=0; i<arms.length; i++) {
arms[i].update();
arms[i].draw();
}
println(frameRate);
}
void mousePressed() {
mouseDragged();
}
void mouseDragged() {
for(int i=0; i<arms.length; i++) {
arms[i].setGoal(new XY(mouseX, mouseY).subtract(arms[i].origin));
}
//arm.setGoal(new XY(mouseX, mouseY).subtract(arm.origin));
}
public class Arm {
float r1, r2;
XY goal;
XY origin;
XY end;
float theta1, theta2;
PID joint1, joint2;
float velo1, velo2;
float MASS = 5;
float UFRIC = 0.85f;
float nodeScale;
boolean dropAtTarget = false;
Arm(float ir1, float ir2, XY iorigin) {
this.r1 = ir1;
this.r2 = ir2;
this.theta1 = (float)Math.PI/2; // Point straight up
this.theta2 = (float)Math.PI;
this.origin = new XY(iorigin);
goal = new XY(0, -(this.r1+this.r2)/2);
float[] t = IK(goal);
this.theta1 = t[0];
this.theta2 = t[1];
this.nodeScale = 1/12. * sqrt(GRID_SIZE/20.);
//joint1 = new PID(MASS*.005,0.000,MASS * .002,1);
//joint2 = new PID(MASS*.005,0.000,MASS * .002,1);
joint1 = new PID(MASS*.005,0.000,MASS * .005,1);
joint2 = new PID(MASS*.005,0.000,MASS * .005,1);
}
void update() {
// IK and physics
float[] angles = new float[2];
angles = IK(this.goal, -1);
float d1 = joint1.update(this.theta1, angles[0]);
float d2 = joint2.update(this.theta2, angles[1]);
velo1 += d1/MASS;
velo2 += d2/MASS;
velo1 *= UFRIC;
velo2 *= UFRIC;
this.theta1 += velo1;
this.theta2 += velo2;
float t2 = this.theta2 + this.theta1 + PI;
this.end = new XY(this.r1 * cos((float)this.theta1) + this.r2 * cos((float)t2), this.r1 * sin((float)this.theta1) + this.r2 * sin((float)t2));
this.end.translate(this.origin);
}
public void draw() {
// *** Draw arm
pushMatrix();
rectMode(CENTER);
translate((float)this.origin.x, (float)this.origin.y);
// Draw range
/*
noFill();
stroke(0,0,120,50);
ellipse(0,0, 2 * (this.r1 + this.r2), 2 * (this.r1 + this.r2));
ellipse(0,0, 2 * (this.r1 - this.r2), 2 * (this.r1 - this.r2));
*/
// First joint and arm
noStroke();
//line(0,0, this.r1 * cos(this.theta1), this.r1 * sin(this.theta1))
noFill();
stroke(0,0,255,40);
strokeWeight(GRID_SIZE/2);
ellipse(0,0,GRID_SIZE*2, GRID_SIZE*2);
//ellipse(0,0,GRID_SIZE*5, GRID_SIZE*5);
//ellipse(0,0,GRID_SIZE*8, GRID_SIZE*8);
pushMatrix();
fill(150,150,00);
//stroke(0,0,255);
noStroke();
rotate((float)(this.theta1+PI/2));
beginShape();
vertex((float)(-this.r1 * this.nodeScale / 2),0);
vertex((float)(-this.r2 * this.nodeScale / 2), -this.r1);
vertex((float)(this.r2 * this.nodeScale / 2), -this.r1);
vertex((float)(this.r1 * this.nodeScale / 2), 0);
endShape(CLOSE);
popMatrix();
//fill(100 - abs(d1)/.05 * 100,100,200);
ellipse(0,0,(float)(this.r1 * this.nodeScale), (float)(this.r1 * this.nodeScale));
// Second joint and arm
pushMatrix();
translate((float)(this.r1 * Math.cos(this.theta1)), (float)(this.r1 * Math.sin(this.theta1)));
noStroke();
float t2 = this.theta2 + this.theta1 + PI;
pushMatrix();
fill(150,150,00);
//stroke(0,0,0);
noStroke();
rotate((float)(t2+Math.PI/2));
beginShape();
vertex((float)(-this.r2 * this.nodeScale / 2), 0);
vertex((float)(-this.r2 * this.nodeScale / 4), -this.r2);
vertex((float)(this.r2 * this.nodeScale / 4), -this.r2);
vertex((float)(this.r2 * this.nodeScale / 2), 0);
endShape(CLOSE);
popMatrix();
//fill(100 - abs(d2)/.05 * 100,100,200);
ellipse(0,0,(float)(this.r2 * this.nodeScale), (float)(this.r2 * this.nodeScale));
// Draw cargo box
float s = 1.1f;
stroke(0,0,0);
strokeWeight(60 * (float)this.nodeScale);
noFill();
rect((float)(this.r2 * Math.cos(t2)-1), (float)(this.r2 * Math.sin(t2)), GRID_SIZE*s, GRID_SIZE*s);
popMatrix();
strokeWeight(1);
this.end = new XY(this.r1 * cos(this.theta1) + this.r2 * cos(t2), this.r1 * sin(this.theta1) + this.r2 * sin(t2));
this.end.translate(this.origin);
// Draw target
pushMatrix();
translate((float)this.goal.x, (float)this.goal.y);
if(this.goal.distance(new XY(this.end.x - this.origin.x, this.end.y - this.origin.y)) > 3) {
noStroke();
fill(0,0,255,(abs(frameCount%40-20) * 8 + 50));
rect(0,0, GRID_SIZE*1.5f, GRID_SIZE*1.5f);
}
popMatrix();
popMatrix();
}
boolean setGoal(XY g) {
if(g.len() < .001)
return false;
else {
this.goal = new XY(g);
return true;
}
}
float[] IK(XY goal) {
return IK(goal, -1);
}
float[] IK(XY goal, int sign) {
goal = new XY(goal);
if(goal.len() > this.r1 + this.r2 ) {
goal.normalize();
goal.scale(.99*(this.r1 + this.r2));
}
if(goal.len() < this.r1 - this.r2 ) {
goal.normalize();
goal.scale(1.01*(this.r1 - this.r2));
}
float[] angles = new float[2];
float dista = goal.len();
if(goal.x < 0) {
sign *= -1;
}
float a = sign * acos(((r1*r1) + pow(dista,2) - r2*r2) / (2 * r1 * dista)) + atan(goal.y / goal.x);
float b = sign * acos((r1*r1 + r2*r2 - dista*dista) / (2 * r1 * r2));
if(a == a && b == b) { angles[0] = a; angles[1] = b; }
if(goal.x < 0) {
angles[0] -= Math.PI;
//angles[1] += PI;
}
return angles;
}
}
public class PID {
public float pK, iK, dK; // Gains
public float pTerm, iTerm, dTerm; // Terms
public float iMax;
public float PIDsum; // pTerm + iTerm + kTerm
public float error;
PID(float p, float i, float d, float iMax) {
this.pK = p;
this.iK = i;
this.dK = d;
this.iMax = iMax;
this.error = 0;
}
float update(float current, float goal) {
float err = goal - current;
this.pTerm = err * this.pK; // error * gain
this.iTerm += err * this.iK; // add error * gain
if(Math.abs(iTerm) > this.iMax)
iTerm = (iTerm < 0 ? -1 : 1) * this.iMax;
this.dTerm = (err - this.error) * this.dK; // changeInError * gain
this.PIDsum = this.pTerm + this.iTerm + this.dTerm;
return this.PIDsum;
}
}
public class XY
{
//NumberFormat f = new DecimalFormat("+000.00;-000.00");
public float x;
public float y;
XY(float ix, float iy) {
this.x = ix;
this.y = iy;
}
XY(XY p) {
this.x = p.x;
this.y = p.y;
}
XY set(float ix, float iy) {
this.x = ix;
this.y = iy;
return this;
}
XY translate(float dx, float dy) {
this.x += dx;
this.y += dy;
return this;
}
XY translate(XY d) {
this.x += d.x;
this.y += d.y;
return this;
}
XY subtract(XY d) {
this.x -= d.x;
this.y -= d.y;
return this;
}
XY subtract(float dx, float dy) {
this.x -= dx;
this.y -= dy;
return this;
}
float distance(XY a) {
return this.distance(this, a);
}
float distance(XY a, XY b) {
a = new XY(a);
a.subtract(b);
return a.len();
}
float distance2(XY a) {
return (pow((this.x - a.x), 2) + pow((this.y - a.y), 2));
}
XY scale(float k) {
this.x *= k;
this.y *= k;
return this;
}
String text() {
return "(" + this.x + ", " + this.y + ")";
}
float len() {
float l = sqrt(this.x*this.x + this.y*this.y);
return l;
}
float length2() {
return this.x*this.x + this.y*this.x;
}
XY normalize() {
if(this.len() > 0) {
this.scale(1/this.len());
}
return this;
}
XY get() {
return new XY(this);
}
boolean equals(XY b) {
if(b.x == this.x && b.y == this.y) return true; else return false;
}
}