Plotclock_v1_2.ino

#include <Servo.h>

#include <Wire.h>

#include <Time.h>

#include <DS1307RTC.h>

// Plotclock

// cc - by Johannes Heberlein 2014

// v 1.01

// thingiverse.com/joo   wiki.fablab-nuernberg.de

// units: mm; microseconds; radians

// origin: bottom left of drawing surface

// time library see http://playground.arduino.cc/Code/time 

// delete or mark the next line as comment when done with calibration  

//#define CALIBRATION

// When in calibration mode, adjust the following factor until the servos move exactly 90 degrees

#define SERVOFAKTOR 750

// Zero-position of left and right servo

// When in calibration mode, adjust the NULL-values so that the servo arms are at all times parallel

// either to the X or Y axis

#define SERVOLEFTNULL 1970

#define SERVORIGHTNULL 840

#define SERVOPINLIFT  2

#define SERVOPINLEFT  3

#define SERVOPINRIGHT 4

// lift positions of lifting servo

#define LIFT0 1080 // on drawing surface

#define LIFT1 925  // between numbers

#define LIFT2 725  // going towards sweeper

// speed of liftimg arm, higher is slower

#define LIFTSPEED 4500

// length of arms

#define L1 35

#define L2 55.1

#define L3 13.2

// origin points of left and right servo 

#define O1X 22

#define O1Y -25

#define O2X 47

#define O2Y -25

#define WISCHEN 111

#define DOPPELPUNKT 11

#include <Time.h> // see http://playground.arduino.cc/Code/time 

#include <Servo.h>

#ifdef REALTIMECLOCK

// for instructions on how to hook up a real time clock,

// see here -> http://www.pjrc.com/teensy/td_libs_DS1307RTC.html

// DS1307RTC works with the DS1307, DS1337 and DS3231 real time clock chips.

// Please run the SetTime example to initialize the time on new RTC chips and begin running.

#include <Wire.h>

#include <DS1307RTC.h> // see http://playground.arduino.cc/Code/time

#endif

int servoLift = 2500;

Servo servo1;  // 

Servo servo2;  // 

Servo servo3;  // 

volatile double lastX = 75;

volatile double lastY = 47.5;

int last_min = 0;

void setup() 

{ 

  // Realtimeclock definieren

  //#ifdef REALTIMECLOCK

  Serial.begin(9600);

  while (!Serial) ; // wait for serial

  delay(200);

  Serial.println("DS1307RTC Read Test");

  Serial.println("-------------------");

  tmElements_t tm;

  if (RTC.read(tm))

  {

    Serial.print("Ok, Time = ");

   /* print2digits(tm.Hour);

    Serial.write(':');

    print2digits(tm.Minute);

    Serial.write(':');

    print2digits(tm.Second);*/

    setTime(tm.Hour,tm.Minute,0,0,0,0);

    Serial.println("DS1307 time is set OK.");

  }

  else

  {

    if (RTC.chipPresent())

    {

      Serial.println("DS1307 is stopped. Please run the SetTime example to initialize the time and begin running.");

    }

    else

    {

      Serial.println("DS1307 read error! Please check the circuitry.");

    }

    }

 // #endif

  // Set current time only the first to values, hh,mm are needed

 // setTime(07,48,0,0,0,0);

  lift(0);

  drawTo(65.2, 35);

  delay(1000);

  servo1.attach(SERVOPINLIFT);  //  lifting servo

  servo2.attach(SERVOPINLEFT);  //  left servo

  servo3.attach(SERVOPINRIGHT);  //  right servo

  delay(1000);

}

void loop() 

{ 

 // CALIBRATION MODE

#ifdef CALIBRATION

  #define STEP 0.001

  char richtung = 1;

  double winkel1 = -90 * PI/180.0;

  double winkel2 = 0 *PI/180.0;

  /*

  // Servohorns will have 90° between movements, parallel to x and y axis

  drawTo(-3, 29.2);

  delay(500);

  drawTo(74.1, 28);

  delay(500);*/

  // Servo auf 90 ° auslegen

  if (richtung) {

 winkel1 += STEP;

 if (winkel1 > M_PI) {

   richtung = 0;

 }

} else {

 winkel2 -= STEP;

 if (winkel2 <= 0) {

   richtung = 1;

 }

}

 servo2.writeMicroseconds(floor(((winkel1) * SERVOFAKTOR)+ SERVOLEFTNULL));

 servo3.writeMicroseconds(floor(((winkel2) * SERVOFAKTOR) + SERVORIGHTNULL));

#else 

  int i = 0;

  if (last_min != minute()) {

    if (!servo1.attached()) servo1.attach(SERVOPINLIFT);

    if (!servo2.attached()) servo2.attach(SERVOPINLEFT);

    if (!servo3.attached()) servo3.attach(SERVOPINRIGHT);

    drawTo(33, 0);

    lift(1);

    delay(100);

    hour();

    while ((i+1)*10 <= hour())

    {

      i++;

    }

    drawTo(33, 0);

    lift(1);

      number(3, 3, WISCHEN, 1);                       // Wischen

      number(15, 28, i, 0.8);                        // x,y,zahl,größe, 1 Zahl für Stunden

      number(28, 28, (hour()-i*10), 0.8);           // x,y,zahl,größe   2 Zahl für Stunden

      number(37, 28, DOPPELPUNKT, 0.8);            // x,y,zahl,größe    Doppelpunkt

    i=0;

    while ((i+1)*10 <= minute())

    {

      i++;

    }

        number(45, 30, i, 0.8);                  // 1 Zahl für Minute

        number(57, 25, (minute()-i*10), 0.8);    // 2 Zahl für Minute

        lift(0);

        drawTo(50, 47.5);                       // Zur Ausgangspostion zurückfahren

        lift(0);

    last_min = minute();

    servo1.detach();

    servo2.detach();

    servo3.detach();

  }

#endif

} 

// Writing numeral with bx by being the bottom left originpoint. Scale 1 equals a 20 mm high font.

// The structure follows this principle: move to first startpoint of the numeral, lift down, draw numeral, lift up

void number(float bx, float by, int num, float scale) {

  switch (num) {

   // Zahlenzeichen Funktion

  case 0:

    drawTo(bx + 12 * scale, by + 6 * scale);

    lift(1);

     bogenGZS(bx + 7 * scale, by + 10 * scale, 10 * scale, -0.8, 6.5, 0.5);

    lift(0);

    break;

  case 1:

    drawTo(bx + 3 * scale, by + 15 * scale);

    lift(1);

    drawTo(bx + 10 * scale, by + 20 * scale);

    drawTo(bx + 10 * scale, by + 0 * scale);

    lift(0);

    break;

  case 2:

    drawTo(bx + 2 * scale, by + 12 * scale);

    lift(1);

    bogenUZS(bx + 8 * scale, by + 14 * scale, 6 * scale, 3, -0.8, 1);

    drawTo(bx + 1 * scale, by + 0 * scale);

    drawTo(bx + 12 * scale, by + 0 * scale);

    lift(0);

    break;

  case 3:

    drawTo(bx + 2 * scale, by + 17 * scale);

    lift(1);

    bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 3, -2, 1);

    bogenUZS(bx + 5 * scale, by + 5 * scale, 5 * scale, 1.57, -3, 1);

    lift(0);

    break;

  case 4:

    drawTo(bx + 10 * scale, by + -2 * scale);

    lift(1);

    drawTo(bx + 12 * scale, by + 18 * scale);

    drawTo(bx + 1 * scale, by + 5 * scale);

    drawTo(bx + 15 * scale, by + 4 * scale);

    lift(0);

    break;

  case 5:

    drawTo(bx + 2 * scale, by + 5 * scale);

    lift(1);

    bogenGZS(bx + 5 * scale, by + 6 * scale, 6 * scale, -2.5, 2, 1); // -2.5 2 1 

    drawTo(bx + 3 * scale, by + 18 * scale);

    drawTo(bx + 15 * scale, by + 10 * scale);

    lift(0);

    break;

  case 6:

    drawTo(bx + 2 * scale, by + 10 * scale);

    lift(1);

    bogenUZS(bx + 7 * scale, by + 6 * scale, 6 * scale, 2, -4.4, 1);

    drawTo(bx + 11 * scale, by + 20 * scale);

    lift(0);

    break;

  case 7:

    drawTo(bx + -3 * scale, by + 20 * scale);

    lift(1);

    drawTo(bx + 15 * scale, by + 20 * scale);

    drawTo(bx + 2 * scale, by + 0);

    lift(0);

    break;

  case 8:

    drawTo(bx + 5 * scale, by + 10 * scale);

    lift(1);

    bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 4.7, -1.8, 1);

    bogenGZS(bx + 3 * scale, by + 6 * scale, 5 * scale, -4.7, 2, 1);

    lift(0);

    break;

  case 9:

    drawTo(bx + 9 * scale, by + 11 * scale);

    lift(1);

    bogenUZS(bx + 7 * scale, by + 15 * scale, 5 * scale, 4, -0.5, 1);

    drawTo(bx + 5 * scale, by + 0);

    lift(0);

    break;

  case WISCHEN:

    lift(1);

    drawTo(85, 35);

    drawTo(5, 49);

    drawTo(75, 35);

    drawTo(3, 49);

    drawTo(75, 25);

    drawTo(3, 38);

    drawTo(5, 40);

    drawTo(5, 35);

    drawTo(65, 35);

    drawTo(65, 30);

     drawTo(7, 35);

     drawTo(8 , 35);

     drawTo(75, 30);

     drawTo(65, 35);

    // drawTo(8, 25);

    // drawTo(65, 35);

  //    drawTo(65, 25);

 //   drawTo(65, 23);

 //   drawTo(25, 20);

  //  drawTo(70, 35);

//    drawTo(35, 1);

   drawTo(24, 8);

   delay(1000);

 //   // drawTo(54, 25);

   // drawTo(5, 20);

    //drawTo(60, 44);

    lift(0);

    break;

  case DOPPELPUNKT:

    drawTo(bx + 5 * scale, by + 15 * scale);

    lift(1);

    bogenGZS(bx + 5 * scale, by + 15 * scale, 0.1 * scale, 1, -1, 1);

    lift(0);

    drawTo(bx + 5 * scale, by + 5 * scale);

    lift(1);

    bogenGZS(bx + 5 * scale, by + 5 * scale, 0.1 * scale, 1, -1, 1);

    lift(0);

    break;

  }

}

void lift(char lift) {

  switch (lift) {

    // room to optimize  !

  case 0: //850

      if (servoLift >= LIFT0) {

      while (servoLift >= LIFT0) 

      {

        servoLift--;

        servo1.writeMicroseconds(servoLift);        

        delayMicroseconds(LIFTSPEED);

      }

    } 

    else {

      while (servoLift <= LIFT0) {

        servoLift++;

        servo1.writeMicroseconds(servoLift);

        delayMicroseconds(LIFTSPEED);

      }

    }

    break;

  case 1: //150

    if (servoLift >= LIFT1) {

      while (servoLift >= LIFT1) {

        servoLift--;

        servo1.writeMicroseconds(servoLift);

        delayMicroseconds(LIFTSPEED);

      }

    } 

    else {

      while (servoLift <= LIFT1) {

        servoLift++;

        servo1.writeMicroseconds(servoLift);

        delayMicroseconds(LIFTSPEED);

      }

    }

    break;

  case 2:

    if (servoLift >= LIFT2) {

      while (servoLift >= LIFT2) {

        servoLift--;

        servo1.writeMicroseconds(servoLift);

        delayMicroseconds(LIFTSPEED);

      }

    } 

    else {

      while (servoLift <= LIFT2) {

        servoLift++;

        servo1.writeMicroseconds(servoLift);        

        delayMicroseconds(LIFTSPEED);

      }

    }

    break;

  }

}

//Zeichnet negativer Kreisbögen mittels kurzer drawTo Liniensegmente

void bogenUZS(float bx, float by, float radius, int start, int ende, float sqee) {

  float inkr = -0.05;

  float count = 0;

  do {

    drawTo(sqee * radius * cos(start + count) + bx,

    radius * sin(start + count) + by);

    count += inkr;

  } 

  while ((start + count) > ende);

}

//Zeichnet positiver Kreisbögen mittels kurzer drawTo Liniensegmente

void bogenGZS(float bx, float by, float radius, int start, int ende, float sqee) {

  float inkr = 0.05;

  float count = 0;

  do {

    drawTo(sqee * radius * cos(start + count) + bx,

    radius * sin(start + count) + by);

    count += inkr;

  } 

  while ((start + count) <= ende);

}

//Zeichenfunktion

void drawTo(double pX, double pY) {

  double dx, dy, c;

  int i;

  // dx dy of new point

  dx = pX - lastX;

  dy = pY - lastY;

  //path lenght in mm, times 4 equals 4 steps per mm

  c = floor(4 * sqrt(dx * dx + dy * dy));

  if (c < 1) c = 1;

  for (i = 0; i <= c; i++) {

    // draw line point by point

    set_XY(lastX + (i * dx / c), lastY + (i * dy / c));

  }

  lastX = pX;

  lastY = pY;

}

double return_angle(double a, double b, double c) {

  // cosine rule for angle between c and a

  return acos((a * a + c * c - b * b) / (2 * a * c));

}

//Berechnung der Inverse Kinematik

void set_XY(double Tx, double Ty) 

{

  delay(1);

  double dx, dy, c, a1, a2, Hx, Hy;

  // calculate triangle between pen, servoLeft and arm joint

  // cartesian dx/dy

  dx = Tx - O1X;

  dy = Ty - O1Y;

  // polar lemgth (c) and angle (a1)

  c = sqrt(dx * dx + dy * dy); // 

  a1 = atan2(dy, dx); //

  a2 = return_angle(L1, L2, c);

  servo2.writeMicroseconds(floor(((a2 + a1 - M_PI) * SERVOFAKTOR) + SERVOLEFTNULL));

  // calculate joinr arm point for triangle of the right servo arm

  a2 = return_angle(L2, L1, c);

  Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5°

  Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI);

  // calculate triangle between pen joint, servoRight and arm joint

  dx = Hx - O2X;

  dy = Hy - O2Y;

  c = sqrt(dx * dx + dy * dy);

  a1 = atan2(dy, dx);

  a2 = return_angle(L1, (L2 - L3), c);

 servo3.writeMicroseconds(floor(((a1 - a2) * SERVOFAKTOR) + SERVORIGHTNULL));

}