markerprinter_UPDATE

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);
  }
}