size/length arms is scaled to model
serial connection/sending data to arduino
connected servo to arduino and so far all works fine
on the image the marker follows the purple line, angle values are printed on screen and send serial to arduino (at the moment only one side)
PROCESSING:
markerPrinter mprint1, mprint2;
float move_x, move_y, counter;
int curmillis, prevmillis;
void setup (){
size(200,300);
//setup mprint
mprint1 = new markerPrinter(10, 70, 70, 'l');
mprint2 = new markerPrinter(-10, 70, 70, 'r');
move_x = 0;
//serial
printArray(Serial.list());
myPort = new Serial(this, Serial.list()[0], 9600);
//delay
curmillis = millis ();
prevmillis = curmillis;
}
void draw (){
curmillis = millis ();
if (curmillis - prevmillis > 20){
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, 130+30*sin (radians (i)));
}
move_y = 130+30*sin (radians (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-120, height-50);
text ("right: "+degrees (mprint2.angle_sh), width-120, height-25);
myPort.write(int(degrees(mprint2.angle_sh)));
move_x++;
if (move_x >= width){
//noLoop(); //stop when x = width
move_x = 0;
}
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; //new 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){
//set markervalue's
marker.set (temp_mx-center.x, temp_my-center.y);
//safety position marker.y > position servo.y+10 to avoid collapse marker/servo
if (marker.y < 10){
marker.y = 10;
}
//calculations angles
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;
//calculations needed for displaying
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 ();
}
}
ARDUINO:
#include <Servo.h>
Servo myservo; // create servo object to control a servo
// twelve servo objects can be created on most boards
int pos = 90; // variable to store the servo position
void setup() {
myservo.attach(9); // attaches the servo on pin 9 to the servo object
myservo.write(pos);
Serial.begin(9600);
while (!Serial) {
; // wait for serial port to connect. Needed!
}
}
void loop() {
if (Serial.available() > 0) {
// read the incoming byte:
pos = Serial.read();
myservo.write(pos);
delay(15);
}
}
