/***************************************************************************
four_wire_horizontal_plotter_V2.ino
Code by lingib https://www.instructables.com/
Last update 5 February 2021
------
ABOUT
------
This code is for a 4-wire 3D plotter with an annular ring
------
NOTES
------
- All measurements are from motor M3
- All step lines are on PORTB
- All direction lines are on PORTC
- All motors (that need to be stepped) are stepped simultaneously
- Capital letters are used for all "global" variables.
- The hash tags in the "MENU" represent fractional numbers.
- The "MENU" uses "sticky" value for XY (i.e. the last value is remembered).
- The G-code is assumed to have been generated by "Inkscape" where (0,0) is at the lower-left
----------
COPYRIGHT
----------
This code is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License as published
by the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This software is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License. If
not, see .
***************************************************************************/
// ====================================
// GLOBALS
// ====================================
// ----- Bit set/clear/check/toggle macros
#define SET(x,y) (x |=(1< <- ->"));
Serial.println(F(" Exit = 'E'"));
// ----- flush the buffer
while (Serial.available() > 0) Serial.read();
// ----- control motors with 'A', 'S', 'K', and 'L' keys
char keystroke = ' ';
while (keystroke != 'E') { //press 'E' key to exit
// ----- check for keypress
if (Serial.available() > 0) {
keystroke = (char) Serial.read();
}
// ----- select task
switch (keystroke) {
case 'a':
case 'A': {
// ----- rotate motor3 CW
for (step = 0; step < steps; step++) {
step_motor(3, CW);
}
keystroke = ' '; //otherwise motor will continue to rotate
break;
}
case 's':
case 'S': {
// ------ rotate motor3 CCW
for (step = 0; step < steps; step++) {
step_motor(3, CCW);
}
keystroke = ' ';
break;
}
case 'k':
case 'K': {
// ----- rotate motor4 CW
for (step = 0; step < steps; step++) {
step_motor(4, CW);
}
keystroke = ' ';
break;
}
case 'l':
case 'L': {
// ----- rotate motor4 CCW
for (step = 0; step < steps; step++) {
step_motor(4, CCW);
}
keystroke = ' ';
break;
}
case 'e':
case 'E': {
// ----- exit
Serial.println(F(" "));
Serial.println(F(" Cables tensioned ..."));
keystroke = 'E';
break;
}
// ----- default for keystroke
default: {
break;
}
}
}
// ----- update coordinates
X = 0.0;
Y = 0.0;
LAST_X = 0.0;
LAST_Y = 0.0;
// ----- update step counters
CURRENT_STEPS1 = calc_target_steps1(0, 0); //motor steps to Inkscape (0,0)
CURRENT_STEPS2 = calc_target_steps2(0, 0);
CURRENT_STEPS3 = calc_target_steps3(0, 0);
CURRENT_STEPS4 = calc_target_steps4(0, 0);
}
// ----------------------------------
// T5 target test pattern
// ----------------------------------
if (INPUT_STRING.startsWith("T5")) {
target();
}
}
// ====================================
// MOVE_TO
// ====================================
void move_to(float x, float y) { //x,y are scaled coordinates
// ----- locals
int
X = round(x),
Y = round(y);
// ----- draw a line between these "scaled" coordinates
draw_line(round(LAST_X), round(LAST_Y), X, Y);
// ----- remember current values
LAST_X = x;
LAST_Y = y;
}
// ====================================
// DRAW LINE
// ====================================
/*
This routine is a variation of Bresenham's line algorithm. The formula is
derived in https://www.instructables.com/id/CNC-Drum-Plotter/.
The algorithm automatically maps all "octants" to "octant 0" and
automatically swaps the XY coordinates if dY is greater than dX. A swap
flag determines which motor moves for any combination X,Y inputs. The swap
algorithm is further optimised by realising that dY is always positive
in quadrants 0,1 and that dX is always positive in "quadrants" 0,3.
*/
void draw_line(int x1, int y1, int x2, int y2) { //x1,y1,x2,y2 are scaled values
// ----- locals
int
x = x1, //current X-axis position
y = y1, //current Y-axis position
dy, //line slope
dx,
slope,
longest, //axis lengths
shortest,
maximum,
error, //bresenham thresholds
threshold;
// ----- find longest and shortest axis
dy = y2 - y1; //vertical distance
dx = x2 - x1; //horizontal distance
longest = max(abs(dy), abs(dx)); //longest axis
shortest = min(abs(dy), abs(dx)); //shortest axis
//
// ----- scale Bresenham values by 2*longest
error = -longest; //add offset to so we can test at zero
threshold = 0; //test now done at zero
maximum = (longest << 1); //multiply by two
slope = (shortest << 1); //multiply by two ... slope equals (shortest*2/longest*2)
// ----- initialise the swap flag
/*
The XY axes are automatically swapped by using "longest" in
the "for loop". XYswap is used to decode the motors.
*/
bool XYswap = true; //used for motor decoding
if (abs(dx) >= abs(dy)) XYswap = false;
// ----- pretend we are always in octant 0
/*
The current X-axis and Y-axis positions will now be incremented (decremented) each time
through the loop. These intermediate steps are parsed to the plot(x,y) function which calculates
the number of steps required to reach each of these intermediate coordinates. This effectively
linearises the plotter and eliminates the unwanted curves inherent in string plotters.
*/
for (int i = 0; i < longest; i++) {
// ----- move left/right along X axis
if (XYswap) { //swap
if (dy < 0) {
y--;
} else {
y++;
}
} else { //no swap
if (dx < 0) {
x--;
} else {
x++;
}
}
// ----- move up/down Y axis
error += slope;
if (error > threshold) {
error -= maximum;
// ----- move up/down along Y axis
if (XYswap) { //swap
if (dx < 0) {
x--;
} else {
x++;
}
} else { //no swap
if (dy < 0) {
y--;
} else {
y++;
}
}
}
// ----- plot the next rounded XYZ coordinate
plot(x, y); //multiple plots
}
}
// ====================================
// plot
// ====================================
void plot(int x, int y) { //x,y are scaled coordinates
// ----- locals
float
fx = (float)x,
fy = (float)y;
unsigned long
current_time, //system time
target_time1, //step motor1 when target_time1 reached
target_time2, //step motor2 when target_time2 reached
target_time3,
target_time4;
long
target_steps1, //steps to target from Inkscape (0,0)
target_steps2,
target_steps3,
target_steps4,
steps1, //number steps each motor to take
steps2,
steps3,
steps4;
// ----- calculate steps to target
target_steps1 = calc_target_steps1(fx, fy);
target_steps2 = calc_target_steps2(fx, fy);
target_steps3 = calc_target_steps3(fx, fy);
target_steps4 = calc_target_steps4(fx, fy);
// ----- calculate steps required
steps1 = target_steps1 - CURRENT_STEPS1; // dual purpose ... also loop counters
steps2 = target_steps2 - CURRENT_STEPS2;
steps3 = target_steps3 - CURRENT_STEPS3; // dual purpose ... also loop counters
steps4 = target_steps4 - CURRENT_STEPS4;
// ----- debug
//Serial.print(steps1); Serial.print("\t"); Serial.print(steps2); Serial.print("\t"); Serial.print(steps3); Serial.print("\t"); Serial.println(steps4);
// ----- calculate motor directions
/*
These DIRECTIONS assume that the cables are wound CW on the drums.
Do not change these equations if the cables are wound CCW.
Instead make the changes in the step_motors() function
*/
DIRECTION1 = (steps1 < 0) ? CW : CCW;
DIRECTION2 = (steps2 < 0) ? CW : CCW;
DIRECTION3 = (steps3 < 0) ? CW : CCW;
DIRECTION4 = (steps4 < 0) ? CW : CCW;
// ----- initialise loop counters
steps1 = (long)abs(steps1);
steps2 = (long)abs(steps2);
steps3 = (long)abs(steps3);
steps4 = (long)abs(steps4);
// ----- calculate DELAY1, DELAY2, DELAY3, DELAY4
calc_delays((unsigned long)steps1, (unsigned long)steps2, (unsigned long)steps3, (unsigned long)steps4);
if (DEBUG) {
Serial.println("");
Serial.print(" DIRECTION1: "); Serial.print(DIRECTION1 ? "CW" : "CCW"); Serial.print("\t");
Serial.print(" steps1: "); Serial.print(steps1); Serial.print("\t");
Serial.print(" DELAY1: "); Serial.print(DELAY1); Serial.print("\t");
Serial.print(" time taken: "); Serial.println(DELAY1 * steps1);
Serial.print(" DIRECTION2: "); Serial.print(DIRECTION2 ? "CW" : "CCW"); Serial.print("\t");
Serial.print(" steps2: "); Serial.print(steps2); Serial.print("\t");
Serial.print(" DELAY2: "); Serial.print(DELAY2); Serial.print("\t");
Serial.print(" time taken: "); Serial.println(DELAY2 * steps2);
Serial.print(" DIRECTION3: "); Serial.print(DIRECTION3 ? "CW" : "CCW"); Serial.print("\t");
Serial.print(" steps3: "); Serial.print(steps3); Serial.print("\t");
Serial.print(" DELAY3: "); Serial.print(DELAY3); Serial.print("\t");
Serial.print(" time taken: "); Serial.println(DELAY3 * steps3);
Serial.print(" DIRECTION4: "); Serial.print(DIRECTION4 ? "CW" : "CCW"); Serial.print("\t");
Serial.print(" steps4: "); Serial.print(steps4); Serial.print("\t");
Serial.print(" DELAY4: "); Serial.print(DELAY4); Serial.print("\t");
Serial.print(" time taken: "); Serial.println(DELAY4 * steps4);
}
// ----- update coordinates
LAST_X = (float)x;
LAST_Y = (float)y;
// ----- calculate time to next step
current_time = micros();
target_time1 = current_time + DELAY1;
target_time2 = current_time + DELAY2;
target_time3 = current_time + DELAY3;
target_time4 = current_time + DELAY4;
while ((steps1 > 0) || (steps2 > 0) || (steps3 > 0) || (steps4 > 0)) { //stop when all down-counters equal zero
// ----- prepare motor1
if (steps1 > 0) {
current_time = micros();
if (current_time > target_time1) {
target_time1 = micros() + DELAY1; //set new target time
steps1--; //decrement counter1
STEP_M1 = true; //set flag
}
}
// ----- prepare motor2
if (steps2 > 0) {
current_time = micros();
if (current_time > target_time2) {
target_time2 = micros() + DELAY2; //set new target time
steps2--; //decrement counter2
STEP_M2 = true; //set flag
}
}
// ----- prepare motor3
if (steps3 > 0) {
current_time = micros();
if (current_time > target_time3) {
target_time3 = micros() + DELAY3; //set new target time
steps3--; //decrement counter3
STEP_M3 = true; //set flag
}
}
// ----- prepare motor4
if (steps4 > 0) {
current_time = micros();
if (current_time > target_time4) {
target_time4 = micros() + DELAY4; //set new target time
steps4--; //decrement counter4
STEP_M4 = true; //set flag
}
}
// ----- now step the motors
if (STEP_M1 || STEP_M2 || STEP_M3 || STEP_M4) {
step_motors();
// ----- reset the flags
STEP_M1 = false;
STEP_M2 = false;
STEP_M3 = false;
STEP_M4 = false;
}
}
}
// ====================================
// CALCULATE DIAGONALS
// ====================================
float calc_d1(float x, float y) {
float d1 = sqrt(sq(Xoffset + x) + sq(YMS - Yoffset - y) + sq(Zoffset));
return d1;
}
float calc_d2(float x, float y) {
float d2 = sqrt(sq(XMS - Xoffset - x) + sq(YMS - Yoffset - y) + sq(Zoffset));
return d2;
}
float calc_d3(float x, float y) {
float d3 = sqrt(sq(Xoffset + x) + sq(Yoffset + y) + sq(Zoffset));
return d3;
}
float calc_d4(float x, float y) {
float d4 = sqrt(sq(XMS - Xoffset - x) + sq(Yoffset + y) + sq(Zoffset));
return d4;
}
// ====================================
// CALCULATE TARGET STEPS
// ====================================
long calc_target_steps1(float x, float y) {
float diagonal = calc_d1(x, y);
long target_steps = (long)(diagonal * STEPS_PER_MM);
return target_steps;
}
long calc_target_steps2(float x, float y) {
float diagonal = calc_d2(x, y);
long target_steps = (long)(diagonal * STEPS_PER_MM);
return target_steps;
}
long calc_target_steps3(float x, float y) {
float diagonal = calc_d3(x, y);
long target_steps = (long)(diagonal * STEPS_PER_MM);
return target_steps;
}
long calc_target_steps4(float x, float y) {
float diagonal = calc_d4(x, y);
long target_steps = (long)(diagonal * STEPS_PER_MM);
return target_steps;
}
// ====================================================================
// CALCULATE DELAYS
// Assigns values to DELAY1, DELAY2, DELAY3, DELAY4 ready for next pen move.
// All motors that rotate must finish stepping at the same time.
// https://stackoverflow.com/questions/6145364/sort-4-number-with-few-comparisons
// ====================================================================
void calc_delays(unsigned long steps1, unsigned long steps2, unsigned long steps3, unsigned long steps4) {
// ----- locals
unsigned long rotate_time;
unsigned long arr[][2] = {
{1, steps1}, // [motor][steps]
{2, steps2},
{3, steps3},
{4, steps4}
};
// // ----- print array values BEFORE sort
// Serial.print(arr[0][0]); Serial.print("\t"); Serial.println(arr[0][1]);
// Serial.print(arr[1][0]); Serial.print("\t"); Serial.println(arr[1][1]);
// Serial.print(arr[2][0]); Serial.print("\t"); Serial.println(arr[2][1]);
// Serial.print(arr[3][0]); Serial.print("\t"); Serial.println(arr[3][1]);
// Serial.println("");
// Sort the steps into ascending order while keeping
// the motor number associated with the number of steps.
if (arr[0][1] > arr[1][1]) {
swap(&arr[0][0], &arr[1][0]); // swap the motor numbers
swap(&arr[0][1], &arr[1][1]); // swap the steps
}
if (arr[2][1] > arr[3][1]) {
swap(&arr[2][0], &arr[3][0]);
swap(&arr[2][1], &arr[3][1]);
}
if (arr[0][1] > arr[2][1]) {
swap(&arr[0][0], &arr[2][0]);
swap(&arr[0][1], &arr[2][1]);
}
if (arr[1][1] > arr[3][1]) {
swap(&arr[1][0], &arr[3][0]);
swap(&arr[1][1], &arr[3][1]);
}
if (arr[1][1] > arr[2][1]) {
swap(&arr[1][0], &arr[2][0]);
swap(&arr[1][1], &arr[2][1]);
}
// // ----- print array values AFTER sort
// Serial.print(arr[0][0]); Serial.print("\t"); Serial.println(arr[0][1]);
// Serial.print(arr[1][0]); Serial.print("\t"); Serial.println(arr[1][1]);
// Serial.print(arr[2][0]); Serial.print("\t"); Serial.println(arr[2][1]);
// Serial.print(arr[3][0]); Serial.print("\t"); Serial.println(arr[3][1]);
// Serial.println("");
// ----- calculate the rotate time for all motors
if (arr[3][1] > 0) { // arr[3][1] holds steps_max
rotate_time = arr[3][1] * DELAY_MIN; // rotate_time = steps_max * DELAY_MIN;
} else {
rotate_time = DELAY_MIN;
}
// ----- assign a delay to each motor
for (int index = 0; index < 4; index++) {
switch (arr[index][0]) {
case 1:
if (steps1 > 0) {
DELAY1 = rotate_time / steps1;
} else {
DELAY1 = rotate_time;
}
break;
case 2:
if (steps2 > 0) {
DELAY2 = rotate_time / steps2;
} else {
DELAY2 = rotate_time;
}
break;
case 3:
if (steps3 > 0) {
DELAY3 = rotate_time / steps3;
} else {
DELAY3 = rotate_time;
}
break;
case 4:
if (steps4 > 0) {
DELAY4 = rotate_time / steps4;
} else {
DELAY4 = rotate_time;
}
break;
default:
break;
}
}
}
// =====================================================
// swap()
// =====================================================
void swap(unsigned long *a, unsigned long *b) {
unsigned long temp = *a;
*a = *b;
*b = temp;
}
// =====================================================
// STEP MOTORS (step one or more motors)
// =====================================================
void step_motors() {
// ----- locals
enum {dir1, dir2, dir3, dir4}; //define the bit positions for PORTB
byte patternC = PORTC; //read current state PORTB
enum {step1, step2, step3, step4}; //define the bit positions for PORTC
byte patternB = PORTB; //read current state PORTC
// ----- set motor directions
/*
Adjust each motor direction here
*/
(DIRECTION1 == CCW) ? CLR(patternC, dir1) : SET(patternC, dir1);
(DIRECTION2 == CW) ? CLR(patternC, dir2) : SET(patternC, dir2);
(DIRECTION3 == CW) ? CLR(patternC, dir3) : SET(patternC, dir3);
(DIRECTION4 == CCW) ? CLR(patternC, dir4) : SET(patternC, dir4);
PORTC = patternC;
delayMicroseconds(PULSE_WIDTH); //wait for direction lines to stabilise
// ----- create leading edge of step pulse(s)
(STEP_M1) ? SET(patternB, step1) : CLR(patternB, step1); //prepare step pulse
(STEP_M2) ? SET(patternB, step2) : CLR(patternB, step2);
(STEP_M3) ? SET(patternB, step3) : CLR(patternB, step3); //prepare step pulse
(STEP_M4) ? SET(patternB, step4) : CLR(patternB, step4);
PORTB = patternB; //step the motors
delayMicroseconds(PULSE_WIDTH); //mandatory delay
// ----- create trailing-edge of step-pulse(s)
patternB = CLR(patternB, step1);
patternB = CLR(patternB, step2);
patternB = CLR(patternB, step3);
patternB = CLR(patternB, step4);
PORTB = patternB;
delayMicroseconds(PULSE_WIDTH); //mandatory delay
// ----- update step counters
if (DIRECTION1 && STEP_M1) CURRENT_STEPS1--;
if (!DIRECTION1 && STEP_M1) CURRENT_STEPS1++;
if (DIRECTION2 && STEP_M2) CURRENT_STEPS2--;
if (!DIRECTION2 && STEP_M2) CURRENT_STEPS2++;
if (DIRECTION3 && STEP_M3) CURRENT_STEPS3--;
if (!DIRECTION3 && STEP_M3) CURRENT_STEPS3++;
if (DIRECTION4 && STEP_M4) CURRENT_STEPS4--;
if (!DIRECTION4 && STEP_M4) CURRENT_STEPS4++;
}
//====================================
// STEP MOTOR (step a single motor)
//====================================
void step_motor(int motor, bool dir) {
// ----- locals
enum {dir1, dir2, dir3, dir4}; // define bit positions for A0, A1, A2, A3
enum {step1, step2, step3, step4}; // define bit positions for pins 8, 9, 10, 11
byte patternC = PORTC;
byte patternB = PORTB;
switch (motor) {
case 3:
CLR(patternB, step3);
CLR(patternB, step4);
PORTB = patternB;
(dir == CW) ? CLR(patternC, dir3) : SET(patternC, dir3); //set motor direction
PORTC = patternC;
delayMicroseconds(PULSE_WIDTH); //wait for direction line to stabilise
SET(patternB, step3); //prepare step pulse
PORTB = patternB; //step the motors
delayMicroseconds(PULSE_WIDTH);
CLR(patternB, step3); //generate trailing-edge
PORTB = patternB; //step the motors
delayMicroseconds(DELAY_MIN);
break;
case 4:
CLR(patternB, step3); //prevent motors stepping
CLR(patternB, step4);
PORTB = patternB;
(dir == CW) ? CLR(patternC, dir4) : SET(patternC, dir4); //set motor direction
PORTC = patternC;
delayMicroseconds(PULSE_WIDTH); //wait for direction line to stabilise
SET(patternB, step4); //prepare step pulse
PORTB = patternB; //step the motors
delayMicroseconds(PULSE_WIDTH);
CLR(patternB, step4); //generate trailing-edge
PORTB = patternB; //step the motors
delayMicroseconds(DELAY_MIN);
break;
}
}
//----------------------------------------------------------------------------
// DRAW ARC CLOCKWISE (G02)
//----------------------------------------------------------------------------
void draw_arc_cw(float x, float y, float i, float j) {
// ----- inkscape sometimes produces some crazy values for i,j
if ((i < -100) || (i > 100) || (j < -100) || (j > 100)) {
move_to(x, y);
} else {
// ----- variables
float
thisX = LAST_X, //current unscaled X co-ordinate
thisY = LAST_Y, //current unscaled Y co-ordinate
nextX = x, //next X co-ordinate
nextY = y, //next Y co-ordinate
newX, //interpolated X co-ordinate
newY, //interpolated Y co-ordinate
I = i, //horizontal distance thisX from circle center
J = j, //vertical distance thisY from circle center
circleX = thisX + I, //circle X co-ordinate
circleY = thisY + J, //circle Y co-ordinate
delta_x, //horizontal distance between thisX and nextX
delta_y, //vertical distance between thisY and nextY
chord, //line_length between lastXY and nextXY
radius, //circle radius
alpha, //interior angle of arc
beta, //fraction of alpha
arc, //subtended by alpha
current_angle, //measured CCW from 3 o'clock
next_angle; //measured CCW from 3 o'clock
// ----- calculate arc
delta_x = thisX - nextX;
delta_y = thisY - nextY;
chord = sqrt(delta_x * delta_x + delta_y * delta_y);
radius = sqrt(I * I + J * J);
alpha = 2 * asin(chord / (2 * radius)); //see construction lines
arc = alpha * radius; //radians
// ----- sub-divide alpha
int segments = 1;
if (arc > ARC_MAX) {
segments = (int)(arc / ARC_MAX);
beta = alpha / segments;
} else {
beta = alpha;
}
// ----- calculate current angle
/*
atan2() angles between 0 and PI are CCW +ve from 3 o'clock.
atan2() angles between 2*PI and PI are CW -ve relative to 3 o'clock
*/
current_angle = atan2(-J, -I);
if (current_angle <= 0) current_angle += 2 * PI; //angles now 360..0 degrees CW
// ----- plot intermediate CW co-ordinates
next_angle = current_angle; //initialise angle
for (int segment = 1; segment < segments; segment++) {
next_angle -= beta; //move CW around circle
if (next_angle < 0) next_angle += 2 * PI; //check if angle crosses zero
newX = circleX + radius * cos(next_angle); //standard circle formula
newY = circleY + radius * sin(next_angle);
move_to(newX, newY);
}
// ----- draw final line
move_to(nextX, nextY);
}
}
//----------------------------------------------------------------------------
// DRAW ARC COUNTER-CLOCKWISE (G03)
//----------------------------------------------------------------------------
/*
We know the start and finish co-ordinates which allows us to calculate the
chord length. We can also calculate the radius using the biarc I,J values.
If we bisect the chord the center angle becomes 2*asin(chord/(2*radius)).
The arc length may now be calculated using the formula arc_length = radius*angle.
*/
void draw_arc_ccw(float x, float y, float i, float j) {
// ----- inkscape sometimes produces some crazy values for i,j
if ((i < -100) || (i > 100) || (j < -100) || (j > 100)) {
move_to(x, y);
} else {
// ----- variables
float
thisX = LAST_X, //current unscaled X co-ordinate
thisY = LAST_Y, //current unscaled Y co-ordinate
nextX = x, //next X co-ordinate
nextY = y, //next Y co-ordinate
newX, //interpolated X co-ordinate
newY, //interpolated Y co-ordinate
I = i, //horizontal distance thisX from circle center
J = j, //vertical distance thisY from circle center
circleX = thisX + I, //circle X co-ordinate
circleY = thisY + J, //circle Y co-ordinate
delta_x, //horizontal distance between thisX and nextX
delta_y, //vertical distance between thisY and nextY
chord, //line_length between lastXY and nextXY
radius, //circle radius
alpha, //interior angle of arc
beta, //fraction of alpha
arc, //subtended by alpha
current_angle, //measured CCW from 3 o'clock
next_angle; //measured CCW from 3 o'clock
// ----- calculate arc
delta_x = thisX - nextX;
delta_y = thisY - nextY;
chord = sqrt(delta_x * delta_x + delta_y * delta_y);
radius = sqrt(I * I + J * J);
alpha = 2 * asin(chord / (2 * radius)); //see construction lines
arc = alpha * radius; //radians
// ----- sub-divide alpha
int segments = 1;
if (arc > ARC_MAX) {
segments = (int)(arc / ARC_MAX);
beta = alpha / segments;
} else {
beta = alpha;
}
// ----- calculate current angle
/*
tan2() angles between 0 and PI are CCW +ve from 3 o'clock.
atan2() angles between 2*PI and PI are CW -ve relative to 3 o'clock
*/
current_angle = atan2(-J, -I);
if (current_angle <= 0) current_angle += 2 * PI; //angles now 360..0 degrees CW
// ----- plot intermediate CCW co-ordinates
next_angle = current_angle; //initialise angle
for (int segment = 1; segment < segments; segment++) {
next_angle += beta; //move CCW around circle
if (next_angle > 2 * PI) next_angle -= 2 * PI; //check if angle crosses zero
newX = circleX + radius * cos(next_angle); //standard circle formula
newY = circleY + radius * sin(next_angle);
move_to(newX, newY);
}
// ----- draw final line
move_to(nextX, nextY);
}
}
// ----------------------------------
// pen_up()
// ----------------------------------
void pen_up() {
OCR2B = 140; // 1mS pulse
delay(100); // give pen-lift time to respond
}
// ----------------------------------
// pen_down()
// ----------------------------------
void pen_down() {
OCR2B = 148; // 1mS pulse
delay(100); // give pen-lift time to respond
}
// ====================================
// TARGET test pattern
// ====================================
void target() {
// ----- circle
process(F("G00 X132.258680 Y133.336320"));
process(F("G02 X132.258680 Y133.336320")); // lower the pen
delay(100);
process(F("G02 X122.747569 Y110.374463 I-32.472969 J-0.000001"));
process(F("G02 X99.785713 Y100.863350 I-22.961858 J22.961856"));
process(F("G02 X87.358846 Y103.335208 I-0.000000 J32.472969"));
process(F("G02 X76.823856 Y110.374463 I12.426867 J30.001111"));
process(F("G02 X69.784601 Y120.909452 I22.961857 J22.961856"));
process(F("G02 X67.312744 Y133.336320 I30.001112 J12.426867"));
process(F("G02 X69.784601 Y145.763188 I32.472969 J0.000001"));
process(F("G02 X76.823856 Y156.298177 I30.001112 J-12.426867"));
process(F("G02 X87.358846 Y163.337432 I22.961857 J-22.961856"));
process(F("G02 X99.785713 Y165.809290 I12.426867 J-30.001111"));
process(F("G02 X122.747569 Y156.298177 I-0.000002 J-32.472969"));
process(F("G02 X132.258680 Y133.336320 I-22.961858 J-22.961856"));
process(F("G00 X132.258680 Y133.336320")); // raise the pen
// ----- square
process(F("G00 X150 Y83"));
process(F("G01 X150 Y83")); // lower the pen
process(F("G01 X50 Y83"));
process(F("G00 X50 Y83")); // raise the pen
process(F("G01 X50 Y83")); // lower the pen
process(F("G01 X50 Y183"));
process(F("G00 X50 Y183")); // raise the pen
process(F("G01 X50 Y183")); // lower the pen
process(F("G01 X150 Y183"));
process(F("G00 X150 Y183")); // raise the pen
process(F("G01 X150 Y183")); // lower the pen
process(F("G01 X150 Y83"));
process(F("G00 X150 Y83")); // raise the pen
// ----- diagonal 1
process(F("G00 X50 Y83"));
process(F("G01 X50 Y83")); // lower the pen
process(F("G01 X150 Y183"));
process(F("G00 X150 Y183")); // raise the pen
// ----- diagonal 2
process(F("G00 X150 Y83"));
process(F("G01 X150 Y83")); // lower the pen
process(F("G01 X50 Y183"));
process(F("G00 X50 Y183")); // raise the pen
// ----- home
process(F("G00 X0.0000 Y0.0000"));
}