Rajzoló óra

Kategória: Arduino.

Áttekintés

Igen látványos Arduino alapú alkalmazás a rajzoló óra, angolul plotclock. A következőképpen működik: egy robotkar percenként letörni a rajzfelületet és kirajzolja a pontos időt. Ennek számos továbbgondolt változata létezik: pl. a lézer óra, amikor egy lézerrel rajzolja ki az órát egy olyan felületre, ahol lassan, kb. egy perc alatt halványul el, vagy a homokóra, ami homokba rajzolja a pontos időt.

Számos leírás létezik az összeszereléséről, de valahogy egyiket sem találtam teljesnek. Emiatt (is) döntöttem úgy, hogy elkészítem ezt az oldalt, aminek segítségével lehet, hogy el lehet készíteni nulláról az eszközt, de az is lehet, hogy egy n+1. információforrás lesz, melyekből össze lehet gereblyézni a szükséges dolgokat.

Források

A fontosabb források az interneten:

Szükséges eszközök

A működéséhez szükség van egy Arduino-ra, mely az egészet vezérli, 3 szervó motorra, melyből kettő a robotkart mozgatja 2 dimenzióban, a harmadik pedig az íróeszközt emeli fel és engedi le, magára a robot szerkezetre, egy íróeszközre, és ha tényleg a pontos időt szeretnénk kiírni, akkor célszerűen egy óra modulra is. Az eszköz készletben elérhető áron megvásárolható (ld. fent), viszont ezt magunknak kell összeraknunk, melyhez nagyon korlátozott segítséget kapunk. Ahhoz, hogy működésre bírjuk, néhány további apróságot meg kell vásárolnunk, valamint számos, nem nyilvánvaló lépést kell végrehajtanunk. A https://www.youtube.com/watch?v=2DZSj8b2RGw videó sokat segített nekem az összerakásban. A következőket vásároltam:

  • Egységkészlet, amit a kereső kidob, ha beírjuk azt, hogy plotclock. Ez az alábbiakat tartalmazza:
    • Arduino Uno
    • USB kábel
    • Arduino shield
    • 3 db SG90-es szervó motor
    • Műanyag alkatrészek, melyeket össze kellett szerelni (ezt akár 3D nyomtatóval is el lehet készíteni) + csavarok
  • Letörölhető filc. A tapasztalat szerint a Magyarországon beszerezhető filcek mérete nem megfelelő, a Kínából ingyenes szállítással megrendelhető Deli 6817 típusú filc viszont igen.
  • Ahhoz, hogy igazi óra hatást keltsen, szükség van egy órajel generátorra is, ami nem felejti el az időt. Ehhez az alábbiakat kell megvenni:
    • Órajel generátor (keresőkulcs: DS3231)
    • 2032-es gombelem (keresőkulcs: CR2032 battery)
    • Legalább 4 db anya-anya kábel (keresőkulcs: Arduino female to female cable)

A teljes összeg kb. 40$-ra jön ki.

Összeszerelés

Az összeszerelést valamelyest a fenti oldalakon ábrák segítik. A thingverse.com oldalon található egy plotclock_SUP7.skp fájl, ami az eszköz 3D modellje, és a SketchUp nevű programmal tudjuk megnézni. Ezt innen tölthetjük el: https://www.sketchup.com/download/all. Hasznos lehet akkor, ha nem boldogulunk a képek alapján. (Megjegyzés: korábban a fenti Banggood oldalról be volt linkelve egy fájl, melyben az összeszerelési útmutató volt található, és ebben volt egy exe, aminek segítségével meg lehetett nézni a 3D modellt. Az írás pillanatában ez nem elérhető.)

Az összeszerelés lépései:

  • Vegyük fel a védőfóliát, különösen a rajzfelületről. (Ezt én elmulasztottam, és már összeszerelt állapotában kellett letépkednem.)
  • Kezdjük el összeszerelni az ábrák alapján. A 3D modell segít, ha elakadtunk.
  • A mozgó alkatrészeknél ügyeljünk arra, hogy a csavar megfelelően legyen meghúzva, ne túl erősen. (Én egy picit túlhúztam, aminek következtében nem működött rendesen a kirajzoló. Mivel azon a ponton már csak szoftveres hibára gondoltam, hosszú idő eltelt, mire rájöttem, hogy érdemes a csavarokon picit lazítani a megfelelő eredmény eléréséhez.)

Bekötés

A bekötés sem nyilvánvaló annak, aki még nem foglalkozott Arduino-val. Először helyezzük a bővítőlapot az Arduino-ra, és nyomjuk rá. Erre egyébként pusztán amiatt van szükség, mert az Arduino UNO-nak alapból csak 1 db 5V-os kivezetése van, nekünk viszont 4-re van szükségünk.

Nagyon röviden az Arduino UNO és a bővítőlap felépítéséről. Az Arduino UNO-nak 14 digitális ki-bemenő lába van 0-tól 13-ig számozva, valamint 6 analóg ki-bemenő lába, A0-tól A5-ig számozva. A bővítőlapka mindegyik mellé helyez egy földet (G-vel jelölve) és egy feszültséget (V-vel jelölve). A bővítőlapkán a digitális lábak felül a második egységben vannak, jobbról balra haladva, míg az analóg lábak az alatt, a lapkán középen. A G (Ground, föld) felül van, a V (Voltage, feszültség) középen, az S (Sensor, érzékelő) pedig alul. Kábelekkel kell rákötni az Arduino-ra a 3 szervó motort és az órajel generátort. Elvileg bármelyik földet és feszültséget használhatjuk, de célszerű a mellé szereltre rákötnünk.

Tegyük fel, hogy a már összeszerelt eszköz úgy áll, hogy hozzánk közelebb van a két szervó, azaz a filc tartó a jobb felső sarokban található. Ez esetben a kábelek bekötése:

  • Bal oldali szervó motor
    • Sárga -> Arduino 2
    • Piros ->Arduino V
    • Barna -> Arduino G
  • Jobb oldali szervó motor
    • Sárga -> Arduino 4
    • Piros ->Arduino V
    • Barna -> Arduino G
  • Alsó szervó motor
    • Sárga -> Arduino 3
    • Piros ->Arduino V
    • Barna -> Arduino G
  • Órajel generátor
    • SCL -> Arduino A5
    • SDA -> Arduino A4
    • VCC -> Arduino V
    • GND -> Arduino G

Beállítás

Szükséges szoftver

Töltsük le és telepítsük az Arduino IDE nevű programot a következő oldalról: https://www.arduino.cc/en/Main/Software.

Az óra beállítása

Először az órajel generátort kell beállítani, mivel az ideiglenesen felülírja a főprogramot. Ha tehát ismét be kell állítani a pontos időt, mert az órajel generátor is elfelejti (sajnos ez gyakran bekövetkezik), akkor végre kell hajtani az itt leírtakat, majd utána ismét rá kell tölteni a főprogramot.

Az óra beállítása az Arduino -> Óra oldalon is megtalálható. Röviden összefoglalva:

  • Először telepítenünk kell két külső könyvtárat
  • Nyissuk meg az alábbit: File -> Examples -> DS1307RTC (ehhez valószínűleg le kell görgetnünk) -> SetTime
  • Feltöltés
    • Kössük az Arduino-t az USB kábel segítségével valamelyik USB portra.
    • Tools -> Board: válasszik ki ezt: Arduino/Genuino Uno
    • Tools -> Port: itt válasszuk ki azt a COM portot, melynél zárójelben szerepel az, hogy Arduino/Genuino Uno.
    • Feltöltés indítása: Sketch -> Upload (vagy Ctrl + U)
    • Az aktuális (fordításkori) rendszeridőt tölti fel. Ha mindent jól csináltunk, akkor azt láthatjuk, hogy Done uploading.
    • Ha ellenőrizni szeretnénk a feltöltés sikerességét, akkor nyissuk meg a fenti SetTime melletti ReadTest programot, hasonlóan töltsük fel, indítsuk el a Serial Monitort (Tools -> Serial Monitor), ott a jobboldalt lent középen a 9600 baud legyen kiválasztva. Másodpercenként látni fogjuk a pontos időt és dátumot.

A kirajzoló szoftver beállítása

A kirajzoló szoftver megtalálható a Thingverse fenti oldalon plotclock_v1_03.ino néven. Ezt is használhatjuk, de úgy láttam, hogy bizonyos konstansok nem megfelelőek, valamint a kód minőségével sem voltam kibékülve, így a jobb érthetőség érdekében átírtam, a következőképpen:

/**
 * Plotclock
 * cc - by Johannes Heberlein 2014
 * 
 * thingiverse.com/joo   wiki.fablab-nuernberg.de
 * units: mm; microseconds; radians
 * origin: bottom left of drawing surface
 * 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.
 * 1.04  Improved code quality.
 */
 
#include <TimeLib.h>
#include <Wire.h>
#include <Servo.h>
#include <Time.h>
#include <DS1307RTC.h>
 
//#define CALIBRATION      // enable calibration mode
#define REALTIMECLOCK    // enable real time clock
// 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.
 
#define WISHY 3 // Offset of the Y coordinats of the plate-wisher
 
// Zero-position (0°) of the 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 -140
#define SERVORIGHTNULL 750
 
// Number of units to move one radian (~57,3°) with the left and right servos.
// When in calibration mode, adjust the following factors until the servos move exactly 90 degrees.
#define SERVOFAKTORLEFT 640
#define SERVOFAKTORRIGHT 590
 
// Pen lift heights. Lower number means higher lift. These values should be adjusted.
#define LIFT_DRAWING_SURFACE 1290
#define LIFT_BETWEEN_NUMBERS 1065
#define LIFT_TOWARDS_SWEEPER 760
 
#define LIFT_SPEED 2000 // higher value results slower movement
 
// Length of arms in mm. These should also be checked and adjusted if necessary.
#define LENGTH1 35   // distance between the servo and the first crew (the same for the left and the right servo)
#define LENGTH2 57.2 // distance between the first crew and the pen (for both left and right)
#define LENGTH3 14.2 // distance between the middle crew and the pen
#define LENGTH4 43   // distance between the first and the second crew (for both left and right)
 
// Origin points of the servos. Might be necessary to adjust.
#define LEFT_SERVO_X 22
#define LEFT_SERVO_Y -25
#define RIGHT_SERVO_X 47
#define RIGHT_SERVO_Y -25
 
// Pin numbers of the motors. Change if you are using other pins.
#define SERVOPINLIFT  2
#define SERVOPINLEFT  3
#define SERVOPINRIGHT 4
 
int servoLift = 1500;
 
Servo servo1; // lift servo
Servo servo2; // left servo
Servo servo3; // rigth servo
 
volatile double lastX = 75;
volatile double lastY = 47.5;
 
int lastMinute = 0;
 
/**
 * This function is automatically executed once at start. It sets up the time.
 */
void setup() {
  pinMode(7, INPUT_PULLUP);
  lift(LIFT_DRAWING_SURFACE);
 
#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(21, 34, 0, 0, 0, 0);
  }
#else
  // Set current time only the first to values, hh,mm are needed
  setTime(12, 34, 0, 0, 0, 0);
#endif
 
  lift(LIFT_DRAWING_SURFACE);
  delay(1000);
}
 
/**
 * This function is automatically executed repeatedly. In calibration mode it moves the servos 90°, parallel to x and y axis.
 * In operational mode it checks if the time has been changed since last execution. If yes it erases the panel and draws the actual time in HH:MM format.
 */
void loop() {
 
#ifdef CALIBRATION
 
  if (!servo1.attached()) servo1.attach(SERVOPINLIFT);
  if (!servo2.attached()) servo2.attach(SERVOPINLEFT);
  if (!servo3.attached()) servo3.attach(SERVOPINRIGHT);
 
  lift(LIFT_BETWEEN_NUMBERS);
  drawStraightLine(-3, 29.2);
  delay(500);
  drawStraightLine(74.1, 28);
  delay(500);
 
#else
 
  int i = 0;
  if (lastMinute != minute()) {
    if (!servo1.attached()) servo1.attach(SERVOPINLIFT);
    if (!servo2.attached()) servo2.attach(SERVOPINLEFT);
    if (!servo3.attached()) servo3.attach(SERVOPINRIGHT);
 
    // erase
    lift(LIFT_DRAWING_SURFACE);
    drawCharacter(3, 3, 10, 1);
 
    double scaleOfNumbers = 0.9;
 
    // draw hours
    int actualHour = hour();
    while ((i + 1) * 10 <= actualHour) {i++;}
    drawCharacter(5, 25, '0' + i, scaleOfNumbers);
    drawCharacter(19, 25, '0' + (actualHour - i * 10), scaleOfNumbers);
 
    // draw :
    drawCharacter(28, 25, ':', scaleOfNumbers);
 
    // draw minutes
    i = 0;
    int actualMinute = minute();
    while ((i + 1) * 10 <= actualMinute) {i++;}
    drawCharacter(34, 25, '0' + i, scaleOfNumbers);
    drawCharacter(48, 25, '0' + (actualMinute - i * 10), scaleOfNumbers);
 
    // move back to original position
    lift(LIFT_TOWARDS_SWEEPER);
    drawStraightLine(71.0, 47.2);
    lift(LIFT_BETWEEN_NUMBERS);
 
    // finalization for the next run
    lastMinute = actualMinute;
    delay(580);
    servo1.detach();
    servo2.detach();
    servo3.detach();
  }
 
#endif
}
 
/** 
 *  Writes characters with (originPointX, originPointY) being the bottom left origin point. 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
 *  
 *    @param originPointX     x-coordinate of the bottom left point
 *    @param originPointY     y-coordinate of the bottom left point
 *    @param characterToDraw  ASCII code of the character to draw
 *    @param scale            scale of the character to be drawn
 */
void drawCharacter(float originPointX, float originPointY, char characterToDraw, float scale) {
  switch (characterToDraw) {
    case '0':
      drawStraightLine(originPointX + 12 * scale, originPointY + 6 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 7 * scale, originPointY + 10 * scale, 10 * scale, -0.8, 6.7, 0.5);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '1':
      drawStraightLine(originPointX + 3 * scale, originPointY + 15 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawStraightLine(originPointX + 10 * scale, originPointY + 20 * scale);
      drawStraightLine(originPointX + 10 * scale, originPointY + 0 * scale);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '2':
      drawStraightLine(originPointX + 2 * scale, originPointY + 12 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 8 * scale, originPointY + 14 * scale, 6 * scale, 3, -0.8, 1);
      drawStraightLine(originPointX + 1 * scale, originPointY + 0 * scale);
      drawStraightLine(originPointX + 12 * scale, originPointY + 0 * scale);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '3':
      drawStraightLine(originPointX + 2 * scale, originPointY + 17 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 5 * scale, originPointY + 15 * scale, 5 * scale, 3, -2, 1);
      drawArc(originPointX + 5 * scale, originPointY + 5 * scale, 5 * scale, 1.57, -3, 1);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '4':
      drawStraightLine(originPointX + 10 * scale, originPointY + 0 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawStraightLine(originPointX + 10 * scale, originPointY + 20 * scale);
      drawStraightLine(originPointX + 2 * scale, originPointY + 6 * scale);
      drawStraightLine(originPointX + 12 * scale, originPointY + 6 * scale);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '5':
      drawStraightLine(originPointX + 2 * scale, originPointY + 5 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 5 * scale, originPointY + 6 * scale, 6 * scale, -2.5, 2, 1);
      drawStraightLine(originPointX + 5 * scale, originPointY + 20 * scale);
      drawStraightLine(originPointX + 12 * scale, originPointY + 20 * scale);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '6':
      drawStraightLine(originPointX + 2 * scale, originPointY + 10 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 7 * scale, originPointY + 6 * scale, 6 * scale, 2, -4.4, 1);
      drawStraightLine(originPointX + 11 * scale, originPointY + 20 * scale);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '7':
      drawStraightLine(originPointX + 2 * scale, originPointY + 20 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawStraightLine(originPointX + 12 * scale, originPointY + 20 * scale);
      drawStraightLine(originPointX + 2 * scale, originPointY + 0);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '8':
      drawStraightLine(originPointX + 5 * scale, originPointY + 10 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 5 * scale, originPointY + 15 * scale, 5 * scale, 4.7, -1.6, 1);
      drawArc(originPointX + 5 * scale, originPointY + 5 * scale, 5 * scale, -4.7, 2, 1);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case '9':
      drawStraightLine(originPointX + 9 * scale, originPointY + 11 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 7 * scale, originPointY + 15 * scale, 5 * scale, 4, -0.5, 1);
      drawStraightLine(originPointX + 5 * scale, originPointY + 0);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case ':':
      drawStraightLine(originPointX + 5 * scale, originPointY + 15 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 5 * scale, originPointY + 15 * scale, 0.1 * scale, 1, -1, 1);
      delay(10);
      lift(LIFT_BETWEEN_NUMBERS);
      drawStraightLine(originPointX + 5 * scale, originPointY + 5 * scale);
      lift(LIFT_DRAWING_SURFACE);
      drawArc(originPointX + 5 * scale, originPointY + 5 * scale, 0.1 * scale, 1, -1, 1);
      delay(10);
      lift(LIFT_BETWEEN_NUMBERS);
      break;
 
    case 10: // erase (ASCII 10 is backspace)
      lift(LIFT_DRAWING_SURFACE);
      drawStraightLine(70, 45);
      drawStraightLine(65 - WISHY, 43);
      drawStraightLine(65 - WISHY, 46);
      drawStraightLine(5, 49);
      drawStraightLine(5, 46);
      drawStraightLine(63 - WISHY, 46);
      drawStraightLine(63 - WISHY, 42);
      drawStraightLine(5, 42);
      drawStraightLine(5, 38);
      drawStraightLine(63 - WISHY, 38);
      drawStraightLine(63 - WISHY, 34);
      drawStraightLine(5, 34);
      drawStraightLine(5, 29);
      drawStraightLine(6, 29);
      drawStraightLine(65 - WISHY, 26);
      drawStraightLine(5, 26);
      drawStraightLine(60 - WISHY, 40);
      drawStraightLine(73.2, 44.0);
      lift(LIFT_TOWARDS_SWEEPER);
      break;
  }
}
 
/**
 * Sets the pen lift.
 * 
 *   @param liftValueToReach  the physical value to reach
 */
void lift(int liftValueToReach) {
  while (servoLift != liftValueToReach) {
    if (servoLift >= liftValueToReach) {servoLift--;} else {servoLift++;}
    servo1.writeMicroseconds(servoLift);
    delayMicroseconds(LIFT_SPEED);
  }
}
 
/**
 * Draws an arc.
 * 
 *   @param centerX      x-coordinate of the center of the drawArc
 *   @param centerY      y-coordinate of the center of the drawArc
 *   @param radius       radius of the drawArc from the center point
 *   @param startPhase   start phase of the drawArc, expressed in radians
 *   @param startPhase   end phase of the drawArc, expressed in radians
 *   @param distortion   ratio of the x/y, y considered to be fixed
 * 
 * Examples:
 *   drawArc(10, 20, 5, 0, 2*pi, 1) - draws a full circle of center (10, 20) and radius 5
 *   drawArc(10, 20, 5, 0, pi/2, 1) - draws upper a half of a circle, starting from right to the left
 *   drawArc(10, 20, 5, pi/2, 0, 1) - draws upper a half of a circle, starting from left to the right
 *   drawArc(10, 20, 5, 0, 2*pi, 0.5) - draws a distorted circle whcih width is half of its height
 */
void drawArc(float centerX, float centerY, float radius, int startPhase, int endPhase, float distortion) {
  float increment = 0.05;
  if (startPhase > endPhase) {increment = -0.05;}
  float count = 0;
  do {
    drawStraightLine(distortion * radius * cos(startPhase + count) + centerX, radius * sin(startPhase + count) + centerY);
    count += increment;
  } while (increment > 0 ? (startPhase + count) <= endPhase : (startPhase + count) > endPhase);
}
 
/**
 * Moves the pen to a new position, step-by-step, drawing a straight line.
 * 
 *   @param destinationX  x-coordinate of the destination position
 *   @param destinationY  y-coordinate of the destination position
 */
void drawStraightLine(double destinationX, double destinationY) {
  double differenceX = destinationX - lastX;
  double differenceY = destinationY - lastY;
  int stepsPerMillimeter = 7;
  int numberOfSteps = floor(stepsPerMillimeter * sqrt(differenceX * differenceX + differenceY * differenceY));
 
  if (numberOfSteps < 1) numberOfSteps = 1;
  // draw line point by point
  for (int i = 0; i <= numberOfSteps; i++) {
    setPosition(lastX + (i * differenceX / numberOfSteps), lastY + (i * differenceY / numberOfSteps));
  }
 
  lastX = destinationX;
  lastY = destinationY;
}
 
/**
 * Considering a triangle with edges of lenght a, b and c, calculates the angle between a and c, usually denoted with beta.
 * This is done using cosine rule. To recall the cosine rule: b^2 = a^2 + c^2 - 2 * a * c * cos beta
 * 
 *   @param aLength   length of the edge a
 *   @param bLength   length of the edge b
 *   @param cLength   length of the edge c
 * 
 *   @return          angle between a and c
 */
double calculateBeta(double aLength, double bLength, double cLength) {
  return acos((aLength * aLength + cLength * cLength - bLength * bLength) / (2 * aLength * cLength));
}
 
/**
 * Moves the pen to a new position.
 * 
 *   @param destinationX   x-coordinate of the destination position
 *   @param destinationY   y-coordinate of the destination position
 * 
 * In the source code below an angle is either defined by 3 vertice, specifying them in the variable name,
 * or by direction, where the horizontal is the line between the two servos.
 */
void setPosition(double destinationX, double destinationY) {
  delay(1);
 
  // setting the left servo, according to the destination position
  double leftServoDestinationDistanceX = destinationX - LEFT_SERVO_X;
  double leftServoDestinationDistanceY = destinationY - LEFT_SERVO_Y;
  double leftServoDestinationAbsoluteDistance = sqrt(leftServoDestinationDistanceX * leftServoDestinationDistanceX + leftServoDestinationDistanceY * leftServoDestinationDistanceY);
  double directionOfPenFromLeftServo = atan2(leftServoDestinationDistanceY, leftServoDestinationDistanceX);
  double angle_Pen_LeftServo_LeftScrew = calculateBeta(LENGTH1, LENGTH2, leftServoDestinationAbsoluteDistance);
  double directionOfLeftArm = directionOfPenFromLeftServo + angle_Pen_LeftServo_LeftScrew;
  servo2.writeMicroseconds(floor(directionOfLeftArm * SERVOFAKTORLEFT) + SERVOLEFTNULL);
 
  // calculating the position of the middle screw
  double angle_LeftScrew_Pen_LeftServo = calculateBeta(LENGTH2, LENGTH1, leftServoDestinationAbsoluteDistance);
  double angle_LeftScrew_Pen_MiddleScrew = 0.621; // 0.621 radian ~= 36,5°
  double directionOfMiddleScrewFromPen = directionOfPenFromLeftServo + angle_LeftScrew_Pen_MiddleScrew - angle_LeftScrew_Pen_LeftServo + PI;
  double middleScrewX = destinationX + LENGTH3 * cos(directionOfMiddleScrewFromPen);
  double middleScrewY = destinationY + LENGTH3 * sin(directionOfMiddleScrewFromPen);
 
  // setting the right servo, according to the middle screw position
  double rightServoMiddleScrewDistanceX = middleScrewX - RIGHT_SERVO_X;
  double rightServoMiddleScrewDistanceY = middleScrewY - RIGHT_SERVO_Y;
  double rightServoMiddleScrewAbsoluteDistance = sqrt(rightServoMiddleScrewDistanceX * rightServoMiddleScrewDistanceX + rightServoMiddleScrewDistanceY * rightServoMiddleScrewDistanceY);
  double directionOfMiddleScrewFromRightServo = atan2(rightServoMiddleScrewDistanceY, rightServoMiddleScrewDistanceX);
  double angle_RightScrew_RightServo_MiddleScrew = calculateBeta(LENGTH1, LENGTH4, rightServoMiddleScrewAbsoluteDistance);
  double directionOfRightArm = directionOfMiddleScrewFromRightServo - angle_RightScrew_RightServo_MiddleScrew;
  servo3.writeMicroseconds(floor(directionOfRightArm * SERVOFAKTORRIGHT) + SERVORIGHTNULL);
}

Készítsünk egy plotclock_v1_04 nevű könyvtárat, azon belül egy plotclock_v1_04.ino nevű fájlt, és abba másoljuk be a fenti programot.

Első lépésben kalibrálnunk kell. Ehhez módosítanunk kell minimálisan a fenti kódon. Ezt:

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

erre kell cserélnünk:

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

Most töltsük fel a programot a fent megadott módon (Ctrl + U). Ha mindent jól csináltunk, akkor a két kar mozog, kb. 90°-ot, ugyanabba az irányba, de egymástól kb. 90°-os távolságra. Ennek hajszálpontosnak kell lennie, így a kódban addig módosítsuk a következő 4 sorban az értékeket, majd töltsük fel, míg el nem érjük a tökéletes 90°-s mozgást és eltérést:

#define SERVOLEFTNULL -140
#define SERVORIGHTNULL 750
 
#define SERVOFAKTORLEFT 640
#define SERVOFAKTORRIGHT 590

Tehát játsszunk el a közeli értékekkel. Pl. a SERVOFAKTORLEFT esetében próbálkozzunk 630-cal vagy 650-nel is. A kalibráláshoz célszerű leszerelni a karra szerelt karokat, és csak a két kart rajta hagyni. Ha sikerült kalibrálnunk, akkor szereljük vissza a másik két kart, és hagyjuk úgy az értékeket.

Első működés: egyelőre hagyjuk a valós idejű órát, és próbáljunk kiíratni előre megadott időt. A következőképpen módosítsuk a forrást:

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

Ha mindent jól csináltunk, akkor elkezdi letörölni a felületet, megpróbálja kirajzolni az időpontot (12:34), majd a helyére tenni a filcet. Lehetséges (sőt: valószínű), hogy ez nem megy elsőre, és ez esetben kalibrálnunk kell a felemelőt is:

#define LIFT_DRAWING_SURFACE 1290
#define LIFT_BETWEEN_NUMBERS 1065
#define LIFT_TOWARDS_SWEEPER 760

Itt is játsszunk el a közeli értékekkel, hogy a 3 magasság: amikor az időt írja ki, amikor felemeli a tollat két számjegy között és amikor a tollat ráhelyezi a tartójára, az megfelelő legyen. Lényegében ezzel is ugyanúgy el kell játszani, mint a fenti derékszögű mozgatással.

A következő, amit lehetséges, hogy kalibrálni kell, az ez:

#define LENGTH1 35   // distance between the servo and the first crew (the same for the left and the right servo)
#define LENGTH2 57.2 // distance between the first crew and the pen (for both left and right)
#define LENGTH3 14.2 // distance between the middle crew and the pen
#define LENGTH4 43   // distance between the first and the second crew (for both left and right)

ha eltérő a karok mérete, valamint ez:

#define LEFT_SERVO_X 22
#define LEFT_SERVO_Y -25
#define RIGHT_SERVO_X 47
#define RIGHT_SERVO_Y -25

a kezdő pozíció megállapításához. (Ez utóbbira akkor van szükség, ha a filcet nem megfelelően helyezi a helyére.)

Ha elégedettek vagyunk az eredménnyel, akkor beállíthatjuk a valós idejű órát, a következő módosítással:

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

Ha mindent jól csináltunk, akkor az eszköz percenként letörli a rajzoló felületet, és kiírja a pontos időt. Érdemes leválasztani a számítógépről, és független áramforrásra kapcsolni (pl. egy konnektorba dugható USB adapter megteszi, melyet számos eszközhöz adnak, és abba dugni az Arduino USB kábelét), hogy ezzel is azt illusztráljuk: az eszköznek a működése során nincs köze a számítógéphez.

Magyarázat

A kód megértéséhez középiskolás szintű matematikára van szükség. Néhány hasznos információ:

  • A végén a setPosition(x, y) függvény kiszámolja a szervó motorok szükséges állapotát, hogy a filcet az (x, y) koordinátára vigye. Ehhez trigonometrikus és geometriai azonosságokat használ, pl. a cosinus-függvényt, melynek megvalósítása a calculateBeta() függvény.
  • A pont kirajzolás képességével megvalósítja az egyenes szakasz (drawStraightLine()) és a nem teljes körív (drawArc()) kirajzolását, melyeket rövid szakaszok egymásutáni kirajzolásával íri el.
  • A drawCharacter(x, y, character, scale) paraméterül kapja, hogy milyen karaktert kell kirajzolnia, hova és milyen méretben, ez pedig egyenes szakaszok és körívek egymásutánjára bontja, és meghívja a megfelelő függvényeket a megfelelő paraméterekkel. A kiírás elején és a végén gondoskodik a filc letevéséről és felemeléséről, ld. lift() függvény. Egyébként ezen a ponton lehet igazán kiterjeszteni a programot, megvalósítva nemcsak a számjegyek, hanem a betűk és egyéb karakterek kiírását is.
  • A setup() és a loop() a szokásos Arduino függvények, melyek beállítják ill. működtetik a rendszert. A loop() folyamatosan figyeli a pontos időt, és ha az eltér az előzőtől, akkor karakterenként kiíratja azt.
  • Külön említést érdemel még a program elején levő rengeteg konstans, melyet a fent leírtak szerint kell beállítani.
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License