Chris Dick
/
Plotclock
Port of the Arduino based Plotclock, initial commit untested.
main.cpp@0:caca11342d59, 2014-03-21 (annotated)
- Committer:
- TheChrisyd
- Date:
- Fri Mar 21 18:01:22 2014 +0000
- Revision:
- 0:caca11342d59
First commit of port for the Arduino based Plotclock, see: http://mbed.org/forum/mbed/topic/4801/
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TheChrisyd | 0:caca11342d59 | 1 | // Plotclock |
TheChrisyd | 0:caca11342d59 | 2 | // cc - by Johannes Heberlein 2014 ported by Chris Dick |
TheChrisyd | 0:caca11342d59 | 3 | // v 1.0 |
TheChrisyd | 0:caca11342d59 | 4 | // thingiverse.com/joo wiki.fablab-nuernberg.de |
TheChrisyd | 0:caca11342d59 | 5 | |
TheChrisyd | 0:caca11342d59 | 6 | // units: mm; microseconds; radians |
TheChrisyd | 0:caca11342d59 | 7 | // origin: bottom left of drawing surface (see sketchUp file) |
TheChrisyd | 0:caca11342d59 | 8 | |
TheChrisyd | 0:caca11342d59 | 9 | // todo detail m3, info grafik winkel, kalibr |
TheChrisyd | 0:caca11342d59 | 10 | |
TheChrisyd | 0:caca11342d59 | 11 | #include "mbed.h" |
TheChrisyd | 0:caca11342d59 | 12 | |
TheChrisyd | 0:caca11342d59 | 13 | // #define CALIBRATION |
TheChrisyd | 0:caca11342d59 | 14 | |
TheChrisyd | 0:caca11342d59 | 15 | #define SERVOFAKTOR 650 |
TheChrisyd | 0:caca11342d59 | 16 | |
TheChrisyd | 0:caca11342d59 | 17 | // length of arms |
TheChrisyd | 0:caca11342d59 | 18 | #define L1 35 |
TheChrisyd | 0:caca11342d59 | 19 | #define L2 55.1 |
TheChrisyd | 0:caca11342d59 | 20 | #define L3 13.2 |
TheChrisyd | 0:caca11342d59 | 21 | |
TheChrisyd | 0:caca11342d59 | 22 | // origin points of left and right servo |
TheChrisyd | 0:caca11342d59 | 23 | #define O1X 22 |
TheChrisyd | 0:caca11342d59 | 24 | #define O1Y -25 |
TheChrisyd | 0:caca11342d59 | 25 | #define O2X 47 |
TheChrisyd | 0:caca11342d59 | 26 | #define O2Y -25 |
TheChrisyd | 0:caca11342d59 | 27 | // When in calibration mode, adjust the NULL-values so that the servo arms are at all times parallel |
TheChrisyd | 0:caca11342d59 | 28 | // either to the X or Y axis |
TheChrisyd | 0:caca11342d59 | 29 | // Zero-position of left and right servo |
TheChrisyd | 0:caca11342d59 | 30 | #define SERVOLEFTNULL 1890 |
TheChrisyd | 0:caca11342d59 | 31 | #define SERVORIGHTNULL 960 |
TheChrisyd | 0:caca11342d59 | 32 | |
TheChrisyd | 0:caca11342d59 | 33 | // lift positions of lifting servo |
TheChrisyd | 0:caca11342d59 | 34 | #define LIFT0 1080// on drawing surface |
TheChrisyd | 0:caca11342d59 | 35 | #define LIFT1 925// between numbers |
TheChrisyd | 0:caca11342d59 | 36 | #define LIFT2 725 |
TheChrisyd | 0:caca11342d59 | 37 | /* going towards sweeper */ |
TheChrisyd | 0:caca11342d59 | 38 | |
TheChrisyd | 0:caca11342d59 | 39 | /* speed of liftimg arm, higher is slower */ |
TheChrisyd | 0:caca11342d59 | 40 | #define LIFTSPEED 1500 |
TheChrisyd | 0:caca11342d59 | 41 | |
TheChrisyd | 0:caca11342d59 | 42 | /* |
TheChrisyd | 0:caca11342d59 | 43 | servo width: |
TheChrisyd | 0:caca11342d59 | 44 | Servo1: Write: 1080ms; Lift Sweep: 724; Lift Number: 924 |
TheChrisyd | 0:caca11342d59 | 45 | Servo2: 0.65-1.98 |
TheChrisyd | 0:caca11342d59 | 46 | Servo3: 1.1-1.8ms |
TheChrisyd | 0:caca11342d59 | 47 | */ |
TheChrisyd | 0:caca11342d59 | 48 | /* |
TheChrisyd | 0:caca11342d59 | 49 | Arduino defines |
TheChrisyd | 0:caca11342d59 | 50 | */ |
TheChrisyd | 0:caca11342d59 | 51 | #define delay(x) (wait_ms(x)) |
TheChrisyd | 0:caca11342d59 | 52 | #define delayMicroseconds(x) (wait_us(x)) |
TheChrisyd | 0:caca11342d59 | 53 | #define M_PI 3.141592653589793238462643 //line 70 math.h |
TheChrisyd | 0:caca11342d59 | 54 | //#include <Time.h> don't need this RTC clock does it |
TheChrisyd | 0:caca11342d59 | 55 | //#include <Servo.h> only using one function, writeMicroseconds(), can be done using period_us() on a PwmOut pin |
TheChrisyd | 0:caca11342d59 | 56 | |
TheChrisyd | 0:caca11342d59 | 57 | |
TheChrisyd | 0:caca11342d59 | 58 | void number( float bx, float by, int num, float scale ) ; |
TheChrisyd | 0:caca11342d59 | 59 | void lift( char lift ) ; |
TheChrisyd | 0:caca11342d59 | 60 | void bogenUZS( float bx, float by, float radius, int start, int ende, float sqee ); |
TheChrisyd | 0:caca11342d59 | 61 | void bogenGZS( float bx, float by, float radius, int start, int ende, float sqee ); |
TheChrisyd | 0:caca11342d59 | 62 | void drawTo(double pX, double pY); |
TheChrisyd | 0:caca11342d59 | 63 | double return_angle( double a, double b, double c ); |
TheChrisyd | 0:caca11342d59 | 64 | void set_XY( double Tx, double Ty ); |
TheChrisyd | 0:caca11342d59 | 65 | int minute( void ); |
TheChrisyd | 0:caca11342d59 | 66 | int hour( void ); |
TheChrisyd | 0:caca11342d59 | 67 | |
TheChrisyd | 0:caca11342d59 | 68 | int servoLift = 1500; |
TheChrisyd | 0:caca11342d59 | 69 | |
TheChrisyd | 0:caca11342d59 | 70 | PwmOut servo1(p21); // lifting servo |
TheChrisyd | 0:caca11342d59 | 71 | PwmOut servo2(p22); // left servo |
TheChrisyd | 0:caca11342d59 | 72 | PwmOut servo3(p23); // right servo |
TheChrisyd | 0:caca11342d59 | 73 | |
TheChrisyd | 0:caca11342d59 | 74 | int val; // variable to read the value from the analog pin |
TheChrisyd | 0:caca11342d59 | 75 | |
TheChrisyd | 0:caca11342d59 | 76 | volatile double lastX = 75; |
TheChrisyd | 0:caca11342d59 | 77 | volatile double lastY = 47.5; |
TheChrisyd | 0:caca11342d59 | 78 | |
TheChrisyd | 0:caca11342d59 | 79 | int last_min = 0; |
TheChrisyd | 0:caca11342d59 | 80 | |
TheChrisyd | 0:caca11342d59 | 81 | |
TheChrisyd | 0:caca11342d59 | 82 | void setup() |
TheChrisyd | 0:caca11342d59 | 83 | { |
TheChrisyd | 0:caca11342d59 | 84 | // Set current time |
TheChrisyd | 0:caca11342d59 | 85 | set_time( (( 2014/*year*/ - 1970) * 31556926 ) |
TheChrisyd | 0:caca11342d59 | 86 | + ( 3/*month*/ * 2629743 ) |
TheChrisyd | 0:caca11342d59 | 87 | + ( 6/*day*/ * 86400 ) |
TheChrisyd | 0:caca11342d59 | 88 | + ( 17/*hour*/ * 3600 ) |
TheChrisyd | 0:caca11342d59 | 89 | + ( 0/*minute*/ * 60 ) |
TheChrisyd | 0:caca11342d59 | 90 | + 0/*second*/ ); |
TheChrisyd | 0:caca11342d59 | 91 | drawTo(75, 44); |
TheChrisyd | 0:caca11342d59 | 92 | lift(0); |
TheChrisyd | 0:caca11342d59 | 93 | /* replace pin assignments with PWM period */ |
TheChrisyd | 0:caca11342d59 | 94 | |
TheChrisyd | 0:caca11342d59 | 95 | servo1.period(0.002); // this is the PWM period used by the Arduino servo library |
TheChrisyd | 0:caca11342d59 | 96 | delay(1000); |
TheChrisyd | 0:caca11342d59 | 97 | } |
TheChrisyd | 0:caca11342d59 | 98 | |
TheChrisyd | 0:caca11342d59 | 99 | void loop() |
TheChrisyd | 0:caca11342d59 | 100 | { |
TheChrisyd | 0:caca11342d59 | 101 | //servo2.period_us(M_PI *SERVOFAKTOR + SERVOLEFTNULL); // was writeMicroseconds |
TheChrisyd | 0:caca11342d59 | 102 | #ifdef CALIBRATION |
TheChrisyd | 0:caca11342d59 | 103 | |
TheChrisyd | 0:caca11342d59 | 104 | // Servohorns will have 90° between movements, parallel to x and y axis |
TheChrisyd | 0:caca11342d59 | 105 | drawTo(-3, 29.2); |
TheChrisyd | 0:caca11342d59 | 106 | delay(500); |
TheChrisyd | 0:caca11342d59 | 107 | drawTo(74.1, 28); |
TheChrisyd | 0:caca11342d59 | 108 | delay(500); |
TheChrisyd | 0:caca11342d59 | 109 | |
TheChrisyd | 0:caca11342d59 | 110 | #else |
TheChrisyd | 0:caca11342d59 | 111 | |
TheChrisyd | 0:caca11342d59 | 112 | |
TheChrisyd | 0:caca11342d59 | 113 | int i = 0; |
TheChrisyd | 0:caca11342d59 | 114 | if (last_min != minute()) { |
TheChrisyd | 0:caca11342d59 | 115 | /* Assuming these are to save power - removed for first run of port */ |
TheChrisyd | 0:caca11342d59 | 116 | //if (!servo1.attached()) servo1.attach(SERVOPINLIFT); |
TheChrisyd | 0:caca11342d59 | 117 | //if (!servo2.attached()) servo2.attach(SERVOPINLEFT); |
TheChrisyd | 0:caca11342d59 | 118 | //if (!servo3.attached()) servo3.attach(SERVOPINRIGHT); |
TheChrisyd | 0:caca11342d59 | 119 | |
TheChrisyd | 0:caca11342d59 | 120 | lift(0); |
TheChrisyd | 0:caca11342d59 | 121 | |
TheChrisyd | 0:caca11342d59 | 122 | // hour(); |
TheChrisyd | 0:caca11342d59 | 123 | while ((i+1)*10 <= hour()) |
TheChrisyd | 0:caca11342d59 | 124 | { |
TheChrisyd | 0:caca11342d59 | 125 | i++; |
TheChrisyd | 0:caca11342d59 | 126 | } |
TheChrisyd | 0:caca11342d59 | 127 | |
TheChrisyd | 0:caca11342d59 | 128 | number(3, 3, 111, 1); |
TheChrisyd | 0:caca11342d59 | 129 | number(5, 25, i, 0.9); |
TheChrisyd | 0:caca11342d59 | 130 | number(19, 25, (hour()-i*10), 0.9); |
TheChrisyd | 0:caca11342d59 | 131 | number(28, 25, 11, 0.9); |
TheChrisyd | 0:caca11342d59 | 132 | |
TheChrisyd | 0:caca11342d59 | 133 | i=0; |
TheChrisyd | 0:caca11342d59 | 134 | while ((i+1)*10 <= minute()) |
TheChrisyd | 0:caca11342d59 | 135 | { |
TheChrisyd | 0:caca11342d59 | 136 | i++; |
TheChrisyd | 0:caca11342d59 | 137 | } |
TheChrisyd | 0:caca11342d59 | 138 | number(34, 25, i, 0.9); |
TheChrisyd | 0:caca11342d59 | 139 | number(48, 25, (minute()-i*10), 0.9); |
TheChrisyd | 0:caca11342d59 | 140 | lift(2); |
TheChrisyd | 0:caca11342d59 | 141 | drawTo(75, 47.5); |
TheChrisyd | 0:caca11342d59 | 142 | lift(1); |
TheChrisyd | 0:caca11342d59 | 143 | last_min = minute(); |
TheChrisyd | 0:caca11342d59 | 144 | |
TheChrisyd | 0:caca11342d59 | 145 | //servo1.detach(); |
TheChrisyd | 0:caca11342d59 | 146 | //servo2.detach(); |
TheChrisyd | 0:caca11342d59 | 147 | //servo3.detach(); |
TheChrisyd | 0:caca11342d59 | 148 | } |
TheChrisyd | 0:caca11342d59 | 149 | #endif |
TheChrisyd | 0:caca11342d59 | 150 | } |
TheChrisyd | 0:caca11342d59 | 151 | |
TheChrisyd | 0:caca11342d59 | 152 | int minute( void ) |
TheChrisyd | 0:caca11342d59 | 153 | { |
TheChrisyd | 0:caca11342d59 | 154 | int minutes = 0; |
TheChrisyd | 0:caca11342d59 | 155 | time_t seconds = time(NULL)+ 19800; |
TheChrisyd | 0:caca11342d59 | 156 | minutes = seconds / 60; |
TheChrisyd | 0:caca11342d59 | 157 | return ( minutes % 60 ); |
TheChrisyd | 0:caca11342d59 | 158 | } |
TheChrisyd | 0:caca11342d59 | 159 | |
TheChrisyd | 0:caca11342d59 | 160 | int hour( void ) |
TheChrisyd | 0:caca11342d59 | 161 | { |
TheChrisyd | 0:caca11342d59 | 162 | int hour = 0; |
TheChrisyd | 0:caca11342d59 | 163 | time_t seconds = time(NULL)+ 19800; |
TheChrisyd | 0:caca11342d59 | 164 | hour = seconds / 3600; |
TheChrisyd | 0:caca11342d59 | 165 | return ( hour % 24 ); |
TheChrisyd | 0:caca11342d59 | 166 | } |
TheChrisyd | 0:caca11342d59 | 167 | |
TheChrisyd | 0:caca11342d59 | 168 | |
TheChrisyd | 0:caca11342d59 | 169 | // Writing numeral with bx by being the bottom left originpoint. Scale 1 equals a 20 mm high font. |
TheChrisyd | 0:caca11342d59 | 170 | // The structure follows this principle: move to first startpoint of the numeral, lift down, draw numeral, lift up |
TheChrisyd | 0:caca11342d59 | 171 | void number(float bx, float by, int num, float scale) { |
TheChrisyd | 0:caca11342d59 | 172 | |
TheChrisyd | 0:caca11342d59 | 173 | switch (num) { |
TheChrisyd | 0:caca11342d59 | 174 | |
TheChrisyd | 0:caca11342d59 | 175 | case 0: |
TheChrisyd | 0:caca11342d59 | 176 | drawTo(bx + 12 * scale, by + 6 * scale); |
TheChrisyd | 0:caca11342d59 | 177 | lift(0); |
TheChrisyd | 0:caca11342d59 | 178 | bogenGZS(bx + 7 * scale, by + 10 * scale, 10 * scale, -0.8, 6.7, 0.5); |
TheChrisyd | 0:caca11342d59 | 179 | lift(1); |
TheChrisyd | 0:caca11342d59 | 180 | break; |
TheChrisyd | 0:caca11342d59 | 181 | case 1: |
TheChrisyd | 0:caca11342d59 | 182 | |
TheChrisyd | 0:caca11342d59 | 183 | drawTo(bx + 3 * scale, by + 15 * scale); |
TheChrisyd | 0:caca11342d59 | 184 | lift(0); |
TheChrisyd | 0:caca11342d59 | 185 | drawTo(bx + 10 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 186 | drawTo(bx + 10 * scale, by + 0 * scale); |
TheChrisyd | 0:caca11342d59 | 187 | lift(1); |
TheChrisyd | 0:caca11342d59 | 188 | break; |
TheChrisyd | 0:caca11342d59 | 189 | case 2: |
TheChrisyd | 0:caca11342d59 | 190 | drawTo(bx + 2 * scale, by + 12 * scale); |
TheChrisyd | 0:caca11342d59 | 191 | lift(0); |
TheChrisyd | 0:caca11342d59 | 192 | bogenUZS(bx + 8 * scale, by + 14 * scale, 6 * scale, 3, -0.8, 1); |
TheChrisyd | 0:caca11342d59 | 193 | drawTo(bx + 1 * scale, by + 0 * scale); |
TheChrisyd | 0:caca11342d59 | 194 | drawTo(bx + 12 * scale, by + 0 * scale); |
TheChrisyd | 0:caca11342d59 | 195 | lift(1); |
TheChrisyd | 0:caca11342d59 | 196 | break; |
TheChrisyd | 0:caca11342d59 | 197 | case 3: |
TheChrisyd | 0:caca11342d59 | 198 | drawTo(bx + 2 * scale, by + 17 * scale); |
TheChrisyd | 0:caca11342d59 | 199 | lift(0); |
TheChrisyd | 0:caca11342d59 | 200 | bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 3, -2, 1); |
TheChrisyd | 0:caca11342d59 | 201 | bogenUZS(bx + 5 * scale, by + 5 * scale, 5 * scale, 1.57, -3, 1); |
TheChrisyd | 0:caca11342d59 | 202 | lift(1); |
TheChrisyd | 0:caca11342d59 | 203 | break; |
TheChrisyd | 0:caca11342d59 | 204 | case 4: |
TheChrisyd | 0:caca11342d59 | 205 | drawTo(bx + 10 * scale, by + 0 * scale); |
TheChrisyd | 0:caca11342d59 | 206 | lift(0); |
TheChrisyd | 0:caca11342d59 | 207 | drawTo(bx + 10 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 208 | drawTo(bx + 2 * scale, by + 6 * scale); |
TheChrisyd | 0:caca11342d59 | 209 | drawTo(bx + 12 * scale, by + 6 * scale); |
TheChrisyd | 0:caca11342d59 | 210 | lift(1); |
TheChrisyd | 0:caca11342d59 | 211 | break; |
TheChrisyd | 0:caca11342d59 | 212 | case 5: |
TheChrisyd | 0:caca11342d59 | 213 | drawTo(bx + 2 * scale, by + 5 * scale); |
TheChrisyd | 0:caca11342d59 | 214 | lift(0); |
TheChrisyd | 0:caca11342d59 | 215 | bogenGZS(bx + 5 * scale, by + 6 * scale, 6 * scale, -2.5, 2, 1); |
TheChrisyd | 0:caca11342d59 | 216 | drawTo(bx + 5 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 217 | drawTo(bx + 12 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 218 | lift(1); |
TheChrisyd | 0:caca11342d59 | 219 | break; |
TheChrisyd | 0:caca11342d59 | 220 | case 6: |
TheChrisyd | 0:caca11342d59 | 221 | drawTo(bx + 2 * scale, by + 10 * scale); |
TheChrisyd | 0:caca11342d59 | 222 | lift(0); |
TheChrisyd | 0:caca11342d59 | 223 | bogenUZS(bx + 7 * scale, by + 6 * scale, 6 * scale, 2, -4.4, 1); |
TheChrisyd | 0:caca11342d59 | 224 | drawTo(bx + 11 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 225 | lift(1); |
TheChrisyd | 0:caca11342d59 | 226 | break; |
TheChrisyd | 0:caca11342d59 | 227 | case 7: |
TheChrisyd | 0:caca11342d59 | 228 | drawTo(bx + 2 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 229 | lift(0); |
TheChrisyd | 0:caca11342d59 | 230 | drawTo(bx + 12 * scale, by + 20 * scale); |
TheChrisyd | 0:caca11342d59 | 231 | drawTo(bx + 2 * scale, by + 0); |
TheChrisyd | 0:caca11342d59 | 232 | lift(1); |
TheChrisyd | 0:caca11342d59 | 233 | break; |
TheChrisyd | 0:caca11342d59 | 234 | case 8: |
TheChrisyd | 0:caca11342d59 | 235 | drawTo(bx + 5 * scale, by + 10 * scale); |
TheChrisyd | 0:caca11342d59 | 236 | lift(0); |
TheChrisyd | 0:caca11342d59 | 237 | bogenUZS(bx + 5 * scale, by + 15 * scale, 5 * scale, 4.7, -1.6, 1); |
TheChrisyd | 0:caca11342d59 | 238 | bogenGZS(bx + 5 * scale, by + 5 * scale, 5 * scale, -4.7, 2, 1); |
TheChrisyd | 0:caca11342d59 | 239 | lift(1); |
TheChrisyd | 0:caca11342d59 | 240 | break; |
TheChrisyd | 0:caca11342d59 | 241 | |
TheChrisyd | 0:caca11342d59 | 242 | case 9: |
TheChrisyd | 0:caca11342d59 | 243 | drawTo(bx + 9 * scale, by + 11 * scale); |
TheChrisyd | 0:caca11342d59 | 244 | lift(0); |
TheChrisyd | 0:caca11342d59 | 245 | bogenUZS(bx + 7 * scale, by + 15 * scale, 5 * scale, 4, -0.5, 1); |
TheChrisyd | 0:caca11342d59 | 246 | drawTo(bx + 5 * scale, by + 0); |
TheChrisyd | 0:caca11342d59 | 247 | lift(1); |
TheChrisyd | 0:caca11342d59 | 248 | break; |
TheChrisyd | 0:caca11342d59 | 249 | |
TheChrisyd | 0:caca11342d59 | 250 | case 111: |
TheChrisyd | 0:caca11342d59 | 251 | lift(0); |
TheChrisyd | 0:caca11342d59 | 252 | drawTo(70, 46); |
TheChrisyd | 0:caca11342d59 | 253 | drawTo(65, 43); |
TheChrisyd | 0:caca11342d59 | 254 | |
TheChrisyd | 0:caca11342d59 | 255 | drawTo(65, 49); |
TheChrisyd | 0:caca11342d59 | 256 | drawTo(5, 49); |
TheChrisyd | 0:caca11342d59 | 257 | drawTo(5, 45); |
TheChrisyd | 0:caca11342d59 | 258 | drawTo(65, 45); |
TheChrisyd | 0:caca11342d59 | 259 | drawTo(65, 40); |
TheChrisyd | 0:caca11342d59 | 260 | |
TheChrisyd | 0:caca11342d59 | 261 | drawTo(5, 40); |
TheChrisyd | 0:caca11342d59 | 262 | drawTo(5, 35); |
TheChrisyd | 0:caca11342d59 | 263 | drawTo(65, 35); |
TheChrisyd | 0:caca11342d59 | 264 | drawTo(65, 30); |
TheChrisyd | 0:caca11342d59 | 265 | |
TheChrisyd | 0:caca11342d59 | 266 | drawTo(5, 30); |
TheChrisyd | 0:caca11342d59 | 267 | drawTo(5, 25); |
TheChrisyd | 0:caca11342d59 | 268 | drawTo(65, 25); |
TheChrisyd | 0:caca11342d59 | 269 | drawTo(65, 20); |
TheChrisyd | 0:caca11342d59 | 270 | |
TheChrisyd | 0:caca11342d59 | 271 | drawTo(5, 20); |
TheChrisyd | 0:caca11342d59 | 272 | drawTo(60, 44); |
TheChrisyd | 0:caca11342d59 | 273 | drawTo(77, 44); |
TheChrisyd | 0:caca11342d59 | 274 | drawTo(75.2, 47); |
TheChrisyd | 0:caca11342d59 | 275 | lift(2); |
TheChrisyd | 0:caca11342d59 | 276 | |
TheChrisyd | 0:caca11342d59 | 277 | break; |
TheChrisyd | 0:caca11342d59 | 278 | |
TheChrisyd | 0:caca11342d59 | 279 | case 11: |
TheChrisyd | 0:caca11342d59 | 280 | drawTo(bx + 5 * scale, by + 15 * scale); |
TheChrisyd | 0:caca11342d59 | 281 | lift(0); |
TheChrisyd | 0:caca11342d59 | 282 | bogenGZS(bx + 5 * scale, by + 15 * scale, 0.1 * scale, 1, -1, 1); |
TheChrisyd | 0:caca11342d59 | 283 | lift(1); |
TheChrisyd | 0:caca11342d59 | 284 | drawTo(bx + 5 * scale, by + 5 * scale); |
TheChrisyd | 0:caca11342d59 | 285 | lift(0); |
TheChrisyd | 0:caca11342d59 | 286 | bogenGZS(bx + 5 * scale, by + 5 * scale, 0.1 * scale, 1, -1, 1); |
TheChrisyd | 0:caca11342d59 | 287 | lift(1); |
TheChrisyd | 0:caca11342d59 | 288 | break; |
TheChrisyd | 0:caca11342d59 | 289 | |
TheChrisyd | 0:caca11342d59 | 290 | } |
TheChrisyd | 0:caca11342d59 | 291 | } |
TheChrisyd | 0:caca11342d59 | 292 | |
TheChrisyd | 0:caca11342d59 | 293 | |
TheChrisyd | 0:caca11342d59 | 294 | |
TheChrisyd | 0:caca11342d59 | 295 | void lift(char lift) { |
TheChrisyd | 0:caca11342d59 | 296 | switch (lift) { |
TheChrisyd | 0:caca11342d59 | 297 | // room to optimise ! |
TheChrisyd | 0:caca11342d59 | 298 | |
TheChrisyd | 0:caca11342d59 | 299 | case 0: //850 |
TheChrisyd | 0:caca11342d59 | 300 | |
TheChrisyd | 0:caca11342d59 | 301 | if (servoLift >= LIFT0) { |
TheChrisyd | 0:caca11342d59 | 302 | while (servoLift >= LIFT0) |
TheChrisyd | 0:caca11342d59 | 303 | { |
TheChrisyd | 0:caca11342d59 | 304 | servoLift--; |
TheChrisyd | 0:caca11342d59 | 305 | servo1.period_us(servoLift); |
TheChrisyd | 0:caca11342d59 | 306 | delayMicroseconds(LIFTSPEED); |
TheChrisyd | 0:caca11342d59 | 307 | } |
TheChrisyd | 0:caca11342d59 | 308 | } |
TheChrisyd | 0:caca11342d59 | 309 | else { |
TheChrisyd | 0:caca11342d59 | 310 | while (servoLift <= LIFT0) { |
TheChrisyd | 0:caca11342d59 | 311 | servoLift++; |
TheChrisyd | 0:caca11342d59 | 312 | servo1.period_us(servoLift); |
TheChrisyd | 0:caca11342d59 | 313 | delayMicroseconds(LIFTSPEED); |
TheChrisyd | 0:caca11342d59 | 314 | |
TheChrisyd | 0:caca11342d59 | 315 | } |
TheChrisyd | 0:caca11342d59 | 316 | |
TheChrisyd | 0:caca11342d59 | 317 | } |
TheChrisyd | 0:caca11342d59 | 318 | |
TheChrisyd | 0:caca11342d59 | 319 | break; |
TheChrisyd | 0:caca11342d59 | 320 | |
TheChrisyd | 0:caca11342d59 | 321 | case 1: //150 |
TheChrisyd | 0:caca11342d59 | 322 | |
TheChrisyd | 0:caca11342d59 | 323 | if (servoLift >= LIFT1) { |
TheChrisyd | 0:caca11342d59 | 324 | while (servoLift >= LIFT1) { |
TheChrisyd | 0:caca11342d59 | 325 | servoLift--; |
TheChrisyd | 0:caca11342d59 | 326 | servo1.period_us(servoLift); |
TheChrisyd | 0:caca11342d59 | 327 | delayMicroseconds(LIFTSPEED); |
TheChrisyd | 0:caca11342d59 | 328 | |
TheChrisyd | 0:caca11342d59 | 329 | } |
TheChrisyd | 0:caca11342d59 | 330 | } |
TheChrisyd | 0:caca11342d59 | 331 | else { |
TheChrisyd | 0:caca11342d59 | 332 | while (servoLift <= LIFT1) { |
TheChrisyd | 0:caca11342d59 | 333 | servoLift++; |
TheChrisyd | 0:caca11342d59 | 334 | servo1.period_us(servoLift); |
TheChrisyd | 0:caca11342d59 | 335 | delayMicroseconds(LIFTSPEED); |
TheChrisyd | 0:caca11342d59 | 336 | } |
TheChrisyd | 0:caca11342d59 | 337 | |
TheChrisyd | 0:caca11342d59 | 338 | } |
TheChrisyd | 0:caca11342d59 | 339 | |
TheChrisyd | 0:caca11342d59 | 340 | break; |
TheChrisyd | 0:caca11342d59 | 341 | |
TheChrisyd | 0:caca11342d59 | 342 | case 2: |
TheChrisyd | 0:caca11342d59 | 343 | |
TheChrisyd | 0:caca11342d59 | 344 | if (servoLift >= LIFT2) { |
TheChrisyd | 0:caca11342d59 | 345 | while (servoLift >= LIFT2) { |
TheChrisyd | 0:caca11342d59 | 346 | servoLift--; |
TheChrisyd | 0:caca11342d59 | 347 | servo1.period_us(servoLift); |
TheChrisyd | 0:caca11342d59 | 348 | delayMicroseconds(LIFTSPEED); |
TheChrisyd | 0:caca11342d59 | 349 | } |
TheChrisyd | 0:caca11342d59 | 350 | } |
TheChrisyd | 0:caca11342d59 | 351 | else { |
TheChrisyd | 0:caca11342d59 | 352 | while (servoLift <= LIFT2) { |
TheChrisyd | 0:caca11342d59 | 353 | servoLift++; |
TheChrisyd | 0:caca11342d59 | 354 | servo1.period_us(servoLift); |
TheChrisyd | 0:caca11342d59 | 355 | delayMicroseconds(LIFTSPEED); |
TheChrisyd | 0:caca11342d59 | 356 | } |
TheChrisyd | 0:caca11342d59 | 357 | } |
TheChrisyd | 0:caca11342d59 | 358 | break; |
TheChrisyd | 0:caca11342d59 | 359 | } |
TheChrisyd | 0:caca11342d59 | 360 | } |
TheChrisyd | 0:caca11342d59 | 361 | |
TheChrisyd | 0:caca11342d59 | 362 | |
TheChrisyd | 0:caca11342d59 | 363 | void bogenUZS(float bx, float by, float radius, int start, int ende, float sqee) { |
TheChrisyd | 0:caca11342d59 | 364 | float inkr = -0.05; |
TheChrisyd | 0:caca11342d59 | 365 | float count = 0; |
TheChrisyd | 0:caca11342d59 | 366 | |
TheChrisyd | 0:caca11342d59 | 367 | do { |
TheChrisyd | 0:caca11342d59 | 368 | drawTo(sqee * radius * cos(start + count) + bx, |
TheChrisyd | 0:caca11342d59 | 369 | radius * sin(start + count) + by); |
TheChrisyd | 0:caca11342d59 | 370 | count += inkr; |
TheChrisyd | 0:caca11342d59 | 371 | } |
TheChrisyd | 0:caca11342d59 | 372 | while ((start + count) > ende); |
TheChrisyd | 0:caca11342d59 | 373 | |
TheChrisyd | 0:caca11342d59 | 374 | } |
TheChrisyd | 0:caca11342d59 | 375 | |
TheChrisyd | 0:caca11342d59 | 376 | void bogenGZS(float bx, float by, float radius, int start, int ende, float sqee) { |
TheChrisyd | 0:caca11342d59 | 377 | float inkr = 0.05; |
TheChrisyd | 0:caca11342d59 | 378 | float count = 0; |
TheChrisyd | 0:caca11342d59 | 379 | |
TheChrisyd | 0:caca11342d59 | 380 | do { |
TheChrisyd | 0:caca11342d59 | 381 | drawTo(sqee * radius * cos(start + count) + bx, |
TheChrisyd | 0:caca11342d59 | 382 | radius * sin(start + count) + by); |
TheChrisyd | 0:caca11342d59 | 383 | count += inkr; |
TheChrisyd | 0:caca11342d59 | 384 | } |
TheChrisyd | 0:caca11342d59 | 385 | while ((start + count) <= ende); |
TheChrisyd | 0:caca11342d59 | 386 | } |
TheChrisyd | 0:caca11342d59 | 387 | |
TheChrisyd | 0:caca11342d59 | 388 | |
TheChrisyd | 0:caca11342d59 | 389 | void drawTo(double pX, double pY) { |
TheChrisyd | 0:caca11342d59 | 390 | double dx, dy, c; |
TheChrisyd | 0:caca11342d59 | 391 | int i; |
TheChrisyd | 0:caca11342d59 | 392 | |
TheChrisyd | 0:caca11342d59 | 393 | // dx dy of new point |
TheChrisyd | 0:caca11342d59 | 394 | dx = pX - lastX; |
TheChrisyd | 0:caca11342d59 | 395 | dy = pY - lastY; |
TheChrisyd | 0:caca11342d59 | 396 | //path lenght in mm, times 4 equals 4 steps per mm |
TheChrisyd | 0:caca11342d59 | 397 | c = floor(4 * sqrt(dx * dx + dy * dy)); |
TheChrisyd | 0:caca11342d59 | 398 | |
TheChrisyd | 0:caca11342d59 | 399 | if (c < 1) c = 1; |
TheChrisyd | 0:caca11342d59 | 400 | |
TheChrisyd | 0:caca11342d59 | 401 | for (i = 0; i <= c; i++) { |
TheChrisyd | 0:caca11342d59 | 402 | // draw line point by point |
TheChrisyd | 0:caca11342d59 | 403 | set_XY(lastX + (i * dx / c), lastY + (i * dy / c)); |
TheChrisyd | 0:caca11342d59 | 404 | |
TheChrisyd | 0:caca11342d59 | 405 | } |
TheChrisyd | 0:caca11342d59 | 406 | |
TheChrisyd | 0:caca11342d59 | 407 | lastX = pX; |
TheChrisyd | 0:caca11342d59 | 408 | lastY = pY; |
TheChrisyd | 0:caca11342d59 | 409 | } |
TheChrisyd | 0:caca11342d59 | 410 | |
TheChrisyd | 0:caca11342d59 | 411 | double return_angle(double a, double b, double c) { |
TheChrisyd | 0:caca11342d59 | 412 | // cosine rule for angle between c and a |
TheChrisyd | 0:caca11342d59 | 413 | return acos((a * a + c * c - b * b) / (2 * a * c)); |
TheChrisyd | 0:caca11342d59 | 414 | } |
TheChrisyd | 0:caca11342d59 | 415 | |
TheChrisyd | 0:caca11342d59 | 416 | void set_XY(double Tx, double Ty) |
TheChrisyd | 0:caca11342d59 | 417 | { |
TheChrisyd | 0:caca11342d59 | 418 | delay(1); |
TheChrisyd | 0:caca11342d59 | 419 | double dx, dy, c, a1, a2, Hx, Hy; |
TheChrisyd | 0:caca11342d59 | 420 | |
TheChrisyd | 0:caca11342d59 | 421 | // calculate triangle between pen, servoLeft and arm joint |
TheChrisyd | 0:caca11342d59 | 422 | // cartesian dx/dy |
TheChrisyd | 0:caca11342d59 | 423 | dx = Tx - O1X; |
TheChrisyd | 0:caca11342d59 | 424 | dy = Ty - O1Y; |
TheChrisyd | 0:caca11342d59 | 425 | |
TheChrisyd | 0:caca11342d59 | 426 | // polar lemgth (c) and angle (a1) |
TheChrisyd | 0:caca11342d59 | 427 | c = sqrt(dx * dx + dy * dy); // |
TheChrisyd | 0:caca11342d59 | 428 | a1 = atan2(dy, dx); // |
TheChrisyd | 0:caca11342d59 | 429 | a2 = return_angle(L1, L2, c); |
TheChrisyd | 0:caca11342d59 | 430 | |
TheChrisyd | 0:caca11342d59 | 431 | servo2.period_us(floor(((a2 + a1 - M_PI) * SERVOFAKTOR) + SERVOLEFTNULL)); |
TheChrisyd | 0:caca11342d59 | 432 | |
TheChrisyd | 0:caca11342d59 | 433 | // calculate joinr arm point for triangle of the right servo arm |
TheChrisyd | 0:caca11342d59 | 434 | a2 = return_angle(L2, L1, c); |
TheChrisyd | 0:caca11342d59 | 435 | Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5° |
TheChrisyd | 0:caca11342d59 | 436 | Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI); |
TheChrisyd | 0:caca11342d59 | 437 | |
TheChrisyd | 0:caca11342d59 | 438 | // calculate triangle between pen joint, servoRight and arm joint |
TheChrisyd | 0:caca11342d59 | 439 | dx = Hx - O2X; |
TheChrisyd | 0:caca11342d59 | 440 | dy = Hy - O2Y; |
TheChrisyd | 0:caca11342d59 | 441 | |
TheChrisyd | 0:caca11342d59 | 442 | c = sqrt(dx * dx + dy * dy); |
TheChrisyd | 0:caca11342d59 | 443 | a1 = atan2(dy, dx); |
TheChrisyd | 0:caca11342d59 | 444 | a2 = return_angle(L1, (L2 - L3), c); |
TheChrisyd | 0:caca11342d59 | 445 | |
TheChrisyd | 0:caca11342d59 | 446 | servo3.period_us(floor(((a1 - a2) * SERVOFAKTOR) + SERVORIGHTNULL)); |
TheChrisyd | 0:caca11342d59 | 447 | } |
TheChrisyd | 0:caca11342d59 | 448 | |
TheChrisyd | 0:caca11342d59 | 449 | |
TheChrisyd | 0:caca11342d59 | 450 | |
TheChrisyd | 0:caca11342d59 | 451 | int main() { |
TheChrisyd | 0:caca11342d59 | 452 | setup(); |
TheChrisyd | 0:caca11342d59 | 453 | while(1) { |
TheChrisyd | 0:caca11342d59 | 454 | loop(); |
TheChrisyd | 0:caca11342d59 | 455 | } |
TheChrisyd | 0:caca11342d59 | 456 | } |