new version:
still automatic movement
but instead of a complete ellipsoid movement, the foot hits the ground now (and stays on the ground)
(so that the upper part of the ellipse is still an ellipse, but the lower part of the ellipse is now a line on the floor)
// version: auto version with floor
// see "pass to Arduino" to find your 6 angles
// https://processing.org/examples/reach1.html
// https://discourse.processing.org/t/push-matrix-and-trig-functions-help/16795/4
// -----------------------------------------------------------------------------------
// (left and right as seen from the robot)
// constants for different lengths
final float segLengthUpperPart = 143; // len of 1 segment // uper leg / thigh / Femur
final float segLengthLowerPart = 180; // len of 1 segment // lower leg / shin / Tibia
final float PelvisHeight = 105; // height pelvis plus head
// constants for type of segment
final int none = 0; // = upper segment of leg
final int rightFoot = 1; // lower segment right (left and right as seen from the robot)
final int leftFoot = 2; // lower segment left (left and right as seen from the robot)
// other consts
int floorY = 0;
int offsetXLeftSidePerspective = 40; // offset for the left side (seen from the robot) x
int offsetYLeftSidePerspective = 19; // offset for the left side (seen from the robot) y
//---------------
// Auto movement
float mouseXL, mouseYL;
float mouseXR, mouseYR;
float angleAutoR=0, angleAutoL=PI;
//---------------
// for right leg
float xRight, yRight,
x2Right, y2Right;
boolean lockRight=true;
boolean firstTimeRight=true;
float dxRight, dyRight,
txRight=0, tyRight=0,
angle1Right=0; // pass to Arduino
float angle2Right; // pass to Arduino
float angle3RightFoot; // pass to Arduino
// ----------------
// for left leg
float xLeft, yLeft,
x2Left, y2Left;
boolean lockLeft=true;
boolean firstTimeLeft=true;
float dxLeft, dyLeft,
txLeft=0, tyLeft=0,
angle1Left=0; // pass to Arduino
float angle2Left; // pass to Arduino
float angle3LeftFoot; // pass to Arduino
// ----------------
// other vars
// PGraphics pg; // invisible PGraphics (press any key to see it). It's used to check mouse against red and blue circle.
//--------------------------------------------------------------------------------------
// Core functions
void setup() {
size(1100, 760);
xRight = width/2;
yRight = height/2;
x2Right = xRight;
y2Right = yRight;
xLeft = width/2+offsetXLeftSidePerspective;
yLeft = height/2-offsetYLeftSidePerspective;
x2Left = xLeft;
y2Left = yLeft;
floorY=height-190;
// pg = createGraphics(width, height);
}
void draw() {
background(209);
// decoration and texts
texts();
// main part
//pg.beginDraw();
//pg.background(0);
//pg.stroke(255);
leftLeg();
head();
rightLeg();
// pg.endDraw();
// For testing pg
//if (keyPressed) {
// image(pg, 0, 0);
//}
// Auto movement
float rad1X = 59;
float rad1Y = 29;
mouseXL = cos(angleAutoL) * rad1X+570+offsetXLeftSidePerspective;
mouseYL = sin(angleAutoL) * rad1Y+557-offsetYLeftSidePerspective;
if (mouseYL > floorY-offsetYLeftSidePerspective-8)
mouseYL=floorY-offsetYLeftSidePerspective-8;
mouseXR = cos(angleAutoR) * rad1X+570;
mouseYR = sin(angleAutoR) * rad1Y+557;
if (mouseYR > floorY-8)
mouseYR=floorY-8;
angleAutoL += 0.1;
angleAutoR += 0.1;
stroke(0);
strokeWeight(2);
line(0, floorY,
width, floorY);
}//func draw
// --------------------------------------------------------------------------------------------------------------
// Inputs functions
//void mousePressed() {
// // mouse locked to foot? Via pg
// if (pg.get(mouseX, mouseY) == color(255, 0, 0) ) {
// lockRight=true; //Yes
// return;
// }
// if (pg.get(mouseX, mouseY) == color(0, 0, 255) ) {
// lockLeft=true; //Yes
// return;
// }
//}
//void mouseReleased() {
// // unlock
// lockRight = false;
// lockLeft = false;
//}
// --------------------------------------------------------------------------------------------------------------
// Leg functions and segment
void rightLeg() {
// the first time we use pre-defined values
if (firstTimeRight) {
dxRight=width/2-30;
dyRight=height;
angle1Right= atan2(dyRight, dxRight);
txRight = width/2-30 - cos(angle1Right) * segLengthLowerPart;
tyRight = height - sin(angle1Right) * segLengthLowerPart;
}
// when mouse is hold, we use its values
if (lockRight) {
dxRight = mouseXR - xRight;
dyRight = mouseYR - yRight;
angle1Right = atan2(dyRight, dxRight);
txRight = mouseXR - cos(angle1Right) * segLengthLowerPart;
tyRight = mouseYR - sin(angle1Right) * segLengthLowerPart;
firstTimeRight=false;
}
dxRight = txRight - x2Right;
dyRight = tyRight - y2Right;
angle2Right = atan2(dyRight, dxRight);
xRight = x2Right + cos(angle2Right) * segLengthUpperPart;
yRight = y2Right + sin(angle2Right) * segLengthUpperPart;
segment(xRight, yRight, angle1Right, segLengthLowerPart, rightFoot); // lower part of leg
segment(x2Right, y2Right, angle2Right, segLengthUpperPart, none); // upper part of leg
}
void leftLeg() {
// the first time we use pre-defined values
if (firstTimeLeft) {
dxLeft=width/2-30;
dyLeft=height;
angle1Left= atan2(dyLeft, dxLeft);
txLeft = width/2-30 - cos(angle1Left) * segLengthLowerPart;
tyLeft = height - sin(angle1Left) * segLengthLowerPart;
}
// when mouse is hold, we use its values
if (lockLeft) {
dxLeft = mouseXL - xLeft;
dyLeft = mouseYL - yLeft;
angle1Left = atan2(dyLeft, dxLeft);
angle1Left = constrain(angle1Left, 0, PI);
txLeft = mouseXL - cos(angle1Left) * segLengthLowerPart;
tyLeft = mouseYL - sin(angle1Left) * segLengthLowerPart;
firstTimeLeft=false;
}
dxLeft = txLeft - x2Left;
dyLeft = tyLeft - y2Left;
angle2Left = atan2(dyLeft, dxLeft);
xLeft = x2Left + cos(angle2Left) * segLengthUpperPart;
yLeft = y2Left + sin(angle2Left) * segLengthUpperPart;
segment(xLeft, yLeft, angle1Left, segLengthLowerPart, leftFoot); // lower part of leg
segment(x2Left, y2Left, angle2Left, segLengthUpperPart, none); // upper part of leg
}
void segment(float x, float y,
float a, // angle
float segLength,
int type) {
fill(0);
pushMatrix();
translate(x, y);
rotate(a);
strokeWeight(20.0);
stroke(255, 100); // WHITE
line(0, 0, segLength, 0);
strokeWeight(5.0);
stroke(255, 0, 0, 100); //RED
line(10, 0, segLength-17, 0);
switch (type) {
case none:
break;
case rightFoot: // right lower leg
fill(0, 255, 0); // GREEN
ellipse(segLength, 0,
6, 6);
if (lockRight)ellipse(segLength, 0,
16, 16);
// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3RightFoot=-a;
rect(0, 0,
30, 10);
//pg.pushMatrix();
//pg.translate(x, y);
//pg.rotate(a);
//pg.translate(segLength, 0);
//pg.fill(255, 0, 0); //RED
//pg.ellipse(0, 0, 25, 25);
//pg.popMatrix();
break;
case leftFoot:
fill(0, 255, 0);
ellipse(segLength, 0,
6, 6);
if (lockLeft)ellipse(segLength, 0,
16, 16);
// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3LeftFoot=-a;
rect(0, 0,
30, 10);
//pg.pushMatrix();
//pg.translate(x, y);
//pg.rotate(a);
//pg.translate(segLength, 0);
//pg.fill( 0, 0, 255); // BLUE
//pg.ellipse(0, 0, 25, 25);
//pg.popMatrix();
break;
}//switch
popMatrix();
}
//-----------------------------------------------------------------------------------------------
// Misc functions
void texts() {
fill(0); // Black
text("Automatic version (left and right as seen from the robot) ", width/2-133, 14);
text("Angles: \n"
+"L Upper " + angle2Left +"\n" // upper part
+"L Lower " + angle1Left+"\n" // lower part
+"L Foot " + angle3LeftFoot+"\n\n" // foot part
+"R Upper " + angle2Right+"\n" // upper part
+"R Lower " + angle1Right+"\n" // lower part
+"R Foot " + angle3RightFoot+"\n", // foot part
13, 14);
text("AUTO Version", width/2+210, 127);
}
void head() {
// antenna
antenna(width/2, height/2-30-80 );
antenna(width/2+offsetXLeftSidePerspective, height/2-30-80-offsetYLeftSidePerspective );
// Head
fill(244);
//stroke(0);
noStroke();
rect(width/2, height/2-PelvisHeight-12,
60, PelvisHeight);
pushMatrix();
translate(0, -PelvisHeight+22);
fill(0);
ellipse (width/2+15, height/2-15,
18, 18);
stroke(255, 0, 0); // RED
line(width/2+3, height/2-27,
width/2+43, height/2-27);
line(width/2+43+8, height/2-27,
width/2+43+8-6, height/2-27+14);
popMatrix();
//text
fill(0);
text("I look right",
width/2+111, height/2-2);
}//func
void antenna (int a, int b) {
strokeWeight(2.0);
stroke(88);
line (a, b,
a-30, b-50);
fill(0);
ellipse (a-30, b-50, 6, 6);
}
//
