markerPrinter mprint1, mprint2;
float move_x, move_y, counter;
int curmillis, prevmillis;
void setup (){
size(400,600);
mprint1 = new markerPrinter(70, 250, 250, 'l');
mprint2 = new markerPrinter(-70, 250, 250, 'r');
curmillis = millis ();
prevmillis = curmillis;
move_x = 0;
}
void draw (){
curmillis = millis ();
if (curmillis - prevmillis > 200){
fill (200);
rect (0, 0, width, height);
fill (0);
text (width+"---"+height, 20, height-40);
for (int i = 0; i < width; i++){
stroke (250, 0, 250);
point(i, 400+30*sin (radians (2*i)));
}
move_y = 400+30*sin (radians (2*move_x));
mprint1.update(move_x, move_y);
mprint2.update(move_x, move_y);
mprint1.display();
mprint2.display();
fill (0);
text ("left : "+degrees (mprint1.angle_sh), width-150, height-50);
text ("right: "+degrees (mprint2.angle_sh), width-150, height-25);
move_x++;
if (move_x >= width){
move_x = 0;
clear ();
}
prevmillis = curmillis;
}
}
void mousePressed(){
}
class markerPrinter{
PVector center = new PVector (); //position center acxes
PVector marker = new PVector (); //position marker
PVector arms = new PVector (); //(|arm1|, |arm2|) length arms
PVector arm1 = new PVector (); //position hinge
PVector arm2 = new PVector (); //position marker, caculated, used to draw arm 2
char side; //position servo vs center
float offsetX; //offsetX servo vs centerpoint
float line_sm; //virtual line marker to servo
float angle_m; //angle between X and line_sm
float angle_mh; //angle between line_sm and arm1
float angle_sm; //angle between arm1 and arm2
float angle_sh; //angle between arm1 and X
markerPrinter(float tempoffx, float temparm1, float temparm2, char temps){
offsetX = tempoffx;
center.set(width/2-offsetX, 50);
arms.set(temparm1, temparm2);
side = temps;
}
void update(float temp_mx, float temp_my){
marker.set (temp_mx-center.x, temp_my-center.y);
if (marker.y < 0){
marker.y = 0;
}
line_sm = sqrt (sq (marker.x)+sq (marker.y));
angle_m = acos (marker.x/line_sm);
switch (side){
case 'l':
angle_mh = acos ((sq (arms.y)-sq (arms.x)-sq (line_sm))/(-2*arms.x*line_sm));
angle_sm = acos ((sq (line_sm)-sq (arms.x)-sq (arms.y))/(-2*arms.x*arms.y));
break;
case 'r':
angle_mh = -acos ((sq (arms.y)-sq (arms.x)-sq (line_sm))/(-2*arms.x*line_sm));
angle_sm = -acos ((sq (line_sm)-sq (arms.x)-sq (arms.y))/(-2*arms.x*arms.y));
break;
}
angle_sh = angle_m + angle_mh;
arm1.x = arms.x*cos (angle_sh);
arm1.y = arms.x*sin (angle_sh);
arm2.x = arms.y*cos (angle_sm-(PI-angle_sh));
arm2.y = arms.y*sin (angle_sm-(PI-angle_sh));
}
void display(){
pushMatrix ();
translate (center.x, center.y);
stroke (180);
line (-center.x, 0, width, 0); //X-acxe servo
//line (0, -center.y, 0, height); //Y-acxe servo
//stroke (255, 0, 0, 25);
//line (0, 0, marker.x, marker.y ); //virtual line servomarker
stroke (0, 0, 255);
line (0, 0, arm1.x, arm1.y); //arm1
translate (arm1.x, arm1.y); //hingepoint
line (0, 0, arm2.x, arm2.y); //arm2
popMatrix ();
}
}
Geen opmerkingen:
Een reactie posten