Chris Dick / Mbed 2 deprecated Plotclock

Port of the Arduino based Plotclock, initial commit untested.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }