PlotClock

//#include <Time.h>

// Plotclock
// cc - by Johannes Heberlein 2014
// v 1.02
// 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
// RTC  library see http://playground.arduino.cc/Code/time
//               or http://www.pjrc.com/teensy/td_libs_DS1307RTC.html
// Change log:
// 1.01  Release by joo at https://github.com/9a/plotclock
// 1.02  Additional features implemented by Dave (https://github.com/Dave1001/):
//       - added ability to calibrate servofaktor seperately for left and right servos
//       - added code to support DS1307, DS1337 and DS3231 real time clock chips
//       - see http://www.pjrc.com/teensy/td_libs_DS1307RTC.html for how to hook up the real time clock
// 1.03  Fixed the length bug at the servo2 angle calculation, other fixups


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

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#define NUMFLAKES 10
#define XPOS 0
#define YPOS 1
#define DELTAY 2


#define LOGO16_GLCD_HEIGHT 16
#define LOGO16_GLCD_WIDTH  16

//#define CALIBRATION      // enable calibration mode
//#define REALTIMECLOCK    // enable real time clock

#define WISHY 3 // Offset of the Y coordinats of the plate-wisher

// When in calibration mode, adjust the following factors until the servos move exactly 90 degrees
#define SERVOFAKTORLEFT 570
#define SERVOFAKTORRIGHT 550

// 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 1780
#define SERVORIGHTNULL 990

#define SERVOPINLIFT  2
#define SERVOPINLEFT  3
#define SERVOPINRIGHT 4

#define ZOFF 0
// lift positions of lifting servo
#define LIFT0 1280+90+ZOFF // on drawing surface
#define LIFT1 1550+ZOFF  // between numbers
#define LIFT2 2010+90+ZOFF  // going towards sweeper
#define LIFT4 1400+ZOFF //op dopje
// speed of liftimg arm, higher is slower
#define LIFTSPEED 3000

// length of arms
#define L1 35
#define L2 55.1
#define L3 13.2
#define L4 45

// origin points of left and right servo
#define O1X 24//22
#define O1Y -25
#define O2X 49//47
#define O2Y -25


#define outputA 10
#define outputB 11


#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 = 1500;

Servo servo1;  //
Servo servo2;  //
Servo servo3;  //

volatile double lastX = 75;
volatile double lastY = 47.5;

int last_min = 0;
int calibration = 1;

int counter = 0;
int counter2 = 0;
int aState;
int aLastState;

int knop = 8;
int buttonState = 0;

void setup()
{
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);

  display.clearDisplay();

  display.setTextSize(3);

  display.println("PLOT CLOCK");
  display.display();

  pinMode (outputA, INPUT);
  pinMode (outputB, INPUT);
  Serial.begin(9600);
  aLastState = digitalRead(outputA);

  pinMode(knop, INPUT);



  pinMode(7, INPUT_PULLUP);
#ifdef REALTIMECLOCK
  Serial.begin(9600);
  //while (!Serial) { ; } // wait for serial port to connect. Needed for Leonardo only

  // Set current time only the first to values, hh,mm are needed
  tmElements_t tm;
  if (RTC.read(tm))
  {
    setTime(tm.Hour, tm.Minute, tm.Second, tm.Day, tm.Month, tm.Year);
    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.");
    }
    // Set current time only the first to values, hh,mm are needed



    setTime(13, 25, 0, 0, 0, 0);
  }
#else
  // Set current time only the first to values, hh,mm are needed
  setTime(21, 15, 0, 0, 0, 0);
#endif




  drawTo(75.2, 47);
  lift(0);
  //servo1.attach(SERVOPINLIFT);  //  lifting servo
  //servo2.attach(SERVOPINLEFT);  //  left servo
  //servo3.attach(SERVOPINRIGHT);  //  right servo
  delay(1000);

}

void loop()
{

  buttonState = digitalRead(knop);
  aState = digitalRead(outputA);
  if ( calibration == 1) {



    if (buttonState == HIGH) {

      display.setTextSize(1);
      display.setCursor(0, 0);
      display.clearDisplay();
      display.println("uur ingesteld");
      display.setTextSize(3);
      display.println(counter);
      display.display();
      delay(1000);
      calibration = 2;
    }

    if (aState != aLastState) {
      if (digitalRead(outputB) != aState) {
        counter ++;
        delay(100);
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel het uur in");
        display.setTextSize(3);
        display.println(counter);
        display.display();

        Serial.println(counter);
        delay(20);

      }
      else  {
        counter--;
        delay(100);
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel het uur in");
        display.setTextSize(3);
        display.println(counter);
        display.display();

        Serial.println(counter / 2);
        delay(20);

      }

      if (counter == 24 ) {
        counter = 0;
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel het uur in");
        display.setTextSize(3);
        display.println(counter);
        display.display();


      }
      if (counter == -1 ) {
        counter = 23;
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel het uur in");
        display.setTextSize(3);
        display.println(counter);
        display.display();

      }



    }
  }



  if ( calibration == 2) {
    buttonState = digitalRead(knop);
    if (buttonState == HIGH) {

      display.setTextSize(1);
      display.setCursor(0, 0);
      display.clearDisplay();
      display.println("minuten ingesteld");
      display.setTextSize(3);
      display.println(counter2);
      display.display();
      delay(1000);
      calibration = 3;
    }

    if (aState != aLastState) {
      if (digitalRead(outputB) != aState) {
        counter2 ++;
        delay(50);
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel de minuten in");
        display.setTextSize(3);
        display.println(counter2);
        display.display();

        Serial.println(counter2);
        delay(20);

      }
      else  {
        counter2--;
        delay(50);
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel de minuten in");
        display.setTextSize(3);
        display.println(counter2);
        display.display();

        Serial.println(counter2);
        delay(20);

      }

      if (counter2 == 60 ) {
        counter2 = 0;
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel de minuten in");
        display.setTextSize(3);
        display.println(counter2);
        display.display();

      }
      if (counter2 == -1 ) {
        counter2 = 59;
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.clearDisplay();
        display.println("Stel de minuten in");
        display.setTextSize(3);
        display.println(counter2);
        display.display();
      }



    }

  }
  if ( calibration == 3) {
    delay(50);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.clearDisplay();
    display.println("Tijd ingesteld op");
    display.setTextSize(2);
    display.print(counter);
    display.print(" : ");
    display.println(counter2);

    display.display();





    setTime(counter , counter2, 0, 0, 0, 0);
    delay(2000);
    display.clearDisplay();
    display.display();
    calibration = 4;


  }
  if ( calibration == 4) {


#ifdef CALIBRATION

    if (!servo1.attached()) servo1.attach(SERVOPINLIFT);
    if (!servo2.attached()) servo2.attach(SERVOPINLEFT);
    if (!servo3.attached()) servo3.attach(SERVOPINRIGHT);

    // Servohorns will have 90° between movements, parallel to x and y axis
    drawTo(-3, 29.2);
    delay(500);
    drawTo(74.1, 28);
    delay(500);

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

      lift(0);

      hour();
      while ((i + 1) * 10 <= hour())
      {
        i++;
      }

      number(3, 3, 111, 1);
      number(5, 25, i, 0.9);
      number(19, 25, (hour() - i * 10), 0.9);
      number(28, 25, 11, 0.9);

      i = 0;
      while ((i + 1) * 10 <= minute())
      {
        i++;
      }
      number(34, 25, i, 0.9);
      number(48, 25, (minute() - i * 10), 0.9);
      lift(2);
      drawTo(71.0, 47.2);
      
      lift(4);
      last_min = minute();
      delay(580);
      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) {

    case 0:
      drawTo(bx + 12 * scale, by + 6 * scale);
      lift(0);
      bogenGZS(bx + 7 * scale, by + 10 * scale, 10 * scale, -0.8, 6.7, 0.5);
      lift(1);
      break;
    case 1:

      drawTo(bx + 3 * scale, by + 15 * scale);
      lift(0);
      drawTo(bx + 10 * scale, by + 20 * scale);
      drawTo(bx + 10 * scale, by + 0 * scale);
      lift(1);
      break;
    case 2:
      drawTo(bx + 2 * scale, by + 12 * scale);
      lift(0);
      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(1);
      break;
    case 3:
      drawTo(bx + 2 * scale, by + 17 * scale);
      lift(0);
      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(1);
      break;
    case 4:
      drawTo(bx + 10 * scale, by + 0 * scale);
      lift(0);
      drawTo(bx + 10 * scale, by + 20 * scale);
      drawTo(bx + 2 * scale, by + 6 * scale);
      drawTo(bx + 12 * scale, by + 6 * scale);
      lift(1);
      break;
    case 5:
      drawTo(bx + 2 * scale, by + 5 * scale);
      lift(0);
      bogenGZS(bx + 5 * scale, by + 6 * scale, 6 * scale, -2.5, 2, 1);
      drawTo(bx + 5 * scale, by + 20 * scale);
      drawTo(bx + 12 * scale, by + 20 * scale);
      lift(1);
      break;
    case 6:
      drawTo(bx + 2 * scale, by + 10 * scale);
      lift(0);
      bogenUZS(bx + 7 * scale, by + 6 * scale, 6 * scale, 2, -4.4, 1);
      drawTo(bx + 11 * scale, by + 20 * scale);
      lift(1);
      break;
    case 7:
      drawTo(bx + 2 * scale, by + 20 * scale);
      lift(0);
      drawTo(bx + 12 * scale, by + 20 * scale);
      drawTo(bx + 2 * scale, by + 0);
      lift(1);
      break;
    case 8:
      drawTo(bx + 5 * scale, by + 10 * scale);
      lift(0);
      bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 4.7, -1.6, 1);
      bogenGZS(bx + 5 * scale, by + 5 * scale, 5 * scale, -4.7, 2, 1);
      lift(1);
      break;

    case 9:
      drawTo(bx + 9 * scale, by + 11 * scale);
      lift(0);
      bogenUZS(bx + 7 * scale, by + 15 * scale, 5 * scale, 4, -0.5, 1);
      drawTo(bx + 5 * scale, by + 0);
      lift(1);
      break;

    case 111:

      lift(0);
      drawTo(70, 45);
      drawTo(65 - WISHY, 43);

      drawTo(65 - WISHY, 46);
      drawTo(5, 49);
      drawTo(5, 46);
      drawTo(63 - WISHY, 46);
      drawTo(63 - WISHY, 42);

      drawTo(5, 42);
      drawTo(5, 38);
      drawTo(63 - WISHY, 38);
      drawTo(63 - WISHY, 34);

      drawTo(5, 34);
      drawTo(5, 29);
      drawTo(6, 29);
      drawTo(65 - WISHY, 26);

      drawTo(5, 26);
      drawTo(60 - WISHY, 40);

      drawTo(73.2, 44.0);
      lift(2);

      break;

    case 11: //dubbelpunt
      drawTo(bx + 5 * scale, by + 15 * scale);
      lift(0);
      bogenGZS(bx + 5 * scale, by + 15 * scale, 0.1 * scale, 1, -1, 1);
      delay(10);
      lift(1);
      drawTo(bx + 5 * scale, by + 5 * scale);
      lift(0);
      bogenGZS(bx + 5 * scale, by + 5 * scale, 0.1 * scale, 1, -1, 1);
      delay(0);
      lift(1);
      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;
 case 4: //150

      if (servoLift >= LIFT4) {
        while (servoLift >= LIFT4) {
          servoLift--;
          servo1.writeMicroseconds(servoLift);
          delayMicroseconds(LIFTSPEED);

        }
      }
      else {
        while (servoLift <= LIFT4) {
          servoLift++;
          servo1.writeMicroseconds(servoLift);
          delayMicroseconds(LIFTSPEED);
        }

      }

      break;
      
  }
}


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

}

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


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

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) * SERVOFAKTORLEFT) + 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, L4, c);

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

}