Revision 0:caca11342d59, committed 2014-03-21

Comitter:

TheChrisyd

Date:

Fri Mar 21 18:01:22 2014 +0000

Commit message:

First commit of port for the Arduino based Plotclock, see: http://mbed.org/forum/mbed/topic/4801/

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 21 18:01:22 2014 +0000
@@ -0,0 +1,456 @@
+// Plotclock
+// cc - by Johannes Heberlein 2014 ported by Chris Dick
+// v 1.0
+// thingiverse.com/joo   wiki.fablab-nuernberg.de
+
+// units: mm; microseconds; radians
+// origin: bottom left of drawing surface (see sketchUp file)
+
+// todo detail m3, info grafik winkel, kalibr 
+
+#include "mbed.h"
+
+// #define CALIBRATION
+
+#define SERVOFAKTOR 650
+
+// 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
+// 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
+// Zero-position of left and right servo
+#define SERVOLEFTNULL 1890
+#define SERVORIGHTNULL 960
+
+// 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 1500
+
+/*
+    servo width:
+        Servo1: Write: 1080ms; Lift Sweep: 724; Lift Number: 924 
+        Servo2: 0.65-1.98
+        Servo3: 1.1-1.8ms 
+*/
+/*
+    Arduino defines
+*/
+#define delay(x)                  (wait_ms(x))
+#define delayMicroseconds(x)      (wait_us(x))
+#define M_PI                      3.141592653589793238462643  //line 70 math.h
+//#include <Time.h>   don't need this RTC clock does it
+//#include <Servo.h>  only using one function, writeMicroseconds(), can be done using period_us() on a PwmOut pin
+
+
+void number( float bx, float by, int num, float scale ) ;
+void lift( char lift ) ;
+void bogenUZS( float bx, float by, float radius, int start, int ende, float sqee );
+void bogenGZS( float bx, float by, float radius, int start, int ende, float sqee );
+void drawTo(double pX, double pY);
+double return_angle( double a, double b, double c );
+void set_XY( double Tx, double Ty );
+int minute( void );
+int hour( void );
+
+int servoLift = 1500;
+
+PwmOut servo1(p21);  //   lifting servo
+PwmOut servo2(p22);  //  left servo
+PwmOut servo3(p23);  //  right servo
+
+int val;    // variable to read the value from the analog pin 
+
+volatile double lastX = 75;
+volatile double lastY = 47.5;
+
+int last_min = 0;
+
+
+void setup() 
+{ 
+  // Set current time
+  set_time( (( 2014/*year*/ - 1970) * 31556926 ) 
+             + (  3/*month*/ * 2629743 ) 
+             + (  6/*day*/ * 86400 )
+             + ( 17/*hour*/ * 3600 )
+             + (  0/*minute*/ * 60 )
+             +    0/*second*/ );  
+  drawTo(75, 44);
+  lift(0);
+  /* replace pin assignments with PWM period  */
+ 
+    servo1.period(0.002);  // this is the PWM period used by the Arduino servo library
+    delay(1000);
+} 
+
+void loop() 
+{  
+    //servo2.period_us(M_PI *SERVOFAKTOR + SERVOLEFTNULL); // was writeMicroseconds 
+#ifdef CALIBRATION
+
+    // 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()) {
+    /* Assuming these are to save power - removed for first run of port */
+    //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(75, 47.5);
+    lift(1);
+    last_min = minute();
+    
+    //servo1.detach();
+    //servo2.detach();
+    //servo3.detach();
+  }
+#endif
+} 
+
+int minute( void )
+{
+    int minutes = 0;
+    time_t seconds = time(NULL)+ 19800; 
+    minutes = seconds / 60;
+    return ( minutes % 60 );
+}
+
+int hour( void )
+{
+    int hour = 0;
+    time_t seconds = time(NULL)+ 19800; 
+    hour = seconds / 3600;
+    return ( hour % 24 );
+}
+
+
+// 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, 46);
+    drawTo(65, 43);
+
+    drawTo(65, 49);
+    drawTo(5, 49);
+    drawTo(5, 45);
+    drawTo(65, 45);
+    drawTo(65, 40);
+
+    drawTo(5, 40);
+    drawTo(5, 35);
+    drawTo(65, 35);
+    drawTo(65, 30);
+
+    drawTo(5, 30);
+    drawTo(5, 25);
+    drawTo(65, 25);
+    drawTo(65, 20);
+
+    drawTo(5, 20);
+    drawTo(60, 44);
+    drawTo(77, 44);
+    drawTo(75.2, 47);
+    lift(2);
+
+    break;
+
+  case 11:
+    drawTo(bx + 5 * scale, by + 15 * scale);
+    lift(0);
+    bogenGZS(bx + 5 * scale, by + 15 * scale, 0.1 * scale, 1, -1, 1);
+    lift(1);
+    drawTo(bx + 5 * scale, by + 5 * scale);
+    lift(0);
+    bogenGZS(bx + 5 * scale, by + 5 * scale, 0.1 * scale, 1, -1, 1);
+    lift(1);
+    break;
+
+  }
+}
+
+
+
+void lift(char lift) {
+  switch (lift) {
+        // room to optimise  !
+
+  case 0: //850
+
+      if (servoLift >= LIFT0) {
+      while (servoLift >= LIFT0) 
+      {
+        servoLift--;
+        servo1.period_us(servoLift);                
+        delayMicroseconds(LIFTSPEED);
+      }
+    } 
+    else {
+      while (servoLift <= LIFT0) {
+        servoLift++;
+        servo1.period_us(servoLift);
+        delayMicroseconds(LIFTSPEED);
+
+      }
+
+    }
+
+    break;
+
+  case 1: //150
+
+    if (servoLift >= LIFT1) {
+      while (servoLift >= LIFT1) {
+        servoLift--;
+        servo1.period_us(servoLift);
+        delayMicroseconds(LIFTSPEED);
+
+      }
+    } 
+    else {
+      while (servoLift <= LIFT1) {
+        servoLift++;
+        servo1.period_us(servoLift);
+        delayMicroseconds(LIFTSPEED);
+      }
+
+    }
+
+    break;
+
+  case 2:
+
+    if (servoLift >= LIFT2) {
+      while (servoLift >= LIFT2) {
+        servoLift--;
+        servo1.period_us(servoLift);
+        delayMicroseconds(LIFTSPEED);
+      }
+    } 
+    else {
+      while (servoLift <= LIFT2) {
+        servoLift++;
+        servo1.period_us(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(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));
+}
+
+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.period_us(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.period_us(floor(((a1 - a2) * SERVOFAKTOR) + SERVORIGHTNULL));
+}
+
+
+
+int main() {
+    setup();
+    while(1) {
+        loop();
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Mar 21 18:01:22 2014 +0000
@@ -0,0 +1,1 @@
+http://world3.dev.mbed.org/users/mbed_official/code/mbed/builds/824293ae5e43
\ No newline at end of file