#include <Servo.h>
struct Motor {
int min_angle, max_angle;
Servo servo;
};
#define IMG_W 64
#define IMG_H 64
int STEP = 2;
#define IDLING_MS 4
#define WRITING_MS 8
struct Motor motor_x, motor_y, motor_p;
byte img[IMG_W/8];
struct Motor newMotor(int pin, int a1, int a2) {
struct Motor m;
m.servo.attach(pin);
m.min_angle = a1;
m.max_angle = a2;
return m;
}
void gotoStart() {
motor_p.servo.write(motor_p.max_angle);
delay(15);
motor_x.servo.write(motor_x.min_angle);
motor_y.servo.write(motor_y.min_angle);
delay(IDLING_MS * STEP * IMG_W + 150);
}
void finish() {
motor_p.servo.write(motor_p.max_angle);
delay(15);
motor_x.servo.write(motor_x.min_angle);
motor_y.servo.write(motor_y.max_angle);
delay(IDLING_MS * STEP * IMG_W + 150);
}
void setup() {
Serial.begin(9600);
motor_y = newMotor(2, 50, 180);
motor_x = newMotor(4, 30, 160);
motor_p = newMotor(6, 90, 99);
gotoStart();
}
boolean nextWayIsEmpty(int i) {
for(; i<IMG_W; ++i) {
if(!(img[i/8] & (1 << 7-(i%8)))) continue;
else return false;
}
return true;
}
void printImg() {
gotoStart();
int x = 0;
for(int y=0; y<IMG_H; ++y) {
for(int i=0; i<IMG_W/8; ++i) img[i] = Serial.read();
Serial.write(61);
for(int i=0; i<STEP; ++i) {
motor_x.servo.write(motor_x.min_angle);
motor_y.servo.write(motor_y.min_angle + y*STEP + i);
delay(IDLING_MS * STEP * x + 30);
for(x=0; x<IMG_W; ++x) {
if(nextWayIsEmpty(x)) {
motor_p.servo.write(motor_p.max_angle); delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
break;
}
motor_p.servo.write((img[x/8] & (1 << 7-(x%8))) ? motor_p.min_angle : motor_p.max_angle);
delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
motor_x.servo.write(motor_x.min_angle + x*STEP);
delay(((img[x/8] & (1 << 7-(x%8))) ? WRITING_MS : IDLING_MS) * STEP);
}
motor_p.servo.write(motor_p.max_angle); delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
}
motor_p.servo.write(motor_p.max_angle); delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
}
gotoStart();
Serial.flush();
}
void checkMessage() {
if(Serial.available()) {
delay(10);
byte msg[] = {Serial.read(), Serial.read()};
if(msg[0] == 'P' && msg[1] == 'R') printImg();
if(msg[0] == 'S' && msg[1] == 'T') gotoStart();
if(msg[0] == 'C' && msg[1] == 'L') finish();
if(msg[0] == 'S' && msg[1] == 'Z') STEP = Serial.read();
if(msg[0] == 'P' && msg[1] == 'T') {
if(Serial.read() == '+') {
byte b = Serial.read();
motor_p.min_angle += b-'0';
motor_p.max_angle += b-'0';
} else {
byte b = Serial.read();
motor_p.min_angle -= b-'0';
motor_p.max_angle -= b-'0';
}
}
}
}
void loop() {
checkMessage();
}