// Based on several examples from adafruit learning…
//
// Copyright (c) 2014 Brian Biggs and Lee Hanson
// This program is distributed under the terms of the GNU General Public License
//
// Pen Decorator is intended to run a dremel style drill/carver in a carriage, to decorate the outside
// of the pen barrel for a freshly turned pen (before it gets assembled). We are not responsible for any
// damage you incur from use of this program.
//
// Sept 29, 2014 Version 1.0
// Initial code
//

// Shows how to run three Steppers at once with varying speeds
//
// Requires the Adafruit_Motorshield v2 library
// https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library
// And AccelStepper with AFMotor support
// https://github.com/adafruit/AccelStepper

// This is for Adafruit Motorshield v2 only!
// It ill not work with v1 shields

#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include “utility/Adafruit_PWMServoDriver.h”
#include <Adafruit_RGBLCDShield.h>
#include <Servo.h>

// Drill setting up or down
#define DRILLUP 140
#define DRILLDOWN 160

// Servo pin number
#define SERVO_PIN 10

// Boundary micro switches
#define HOMEPIN 13 // the starting “end” of the decorator
#define AWAYPIN 12 // the ending “end” of the decorator (cause left and right don’t make sense…)

// Directionality for the drive
#define RIGHT 1 // positive for direction
#define LEFT -1 // negative for direction

// These #defines make it easy to set the backlight color
#define OFF 0x0
#define RED 0x1
#define YELLOW 0x3
#define GREEN 0x2
#define TEAL 0x6
#define BLUE 0x4
#define VIOLET 0x5
#define WHITE 0x7

// endpoint offsets (what to add to the switched position when we set the right extent)
#define EXTENT_OFFSET 3000

// variables of boundary switch reads
int hm, aw;

// extents for the decorator
long driveStartPos = 0;
long driveEndPos = 0;
long mandrelZeroPos = 0;

// variables for stepper 1 (mandrel)
int circ = 8;
int spr1 = 200; // # steps to a single rotation
float maxSpeed1 = 100.0;
float accel1 = 100.0;
int amplitude = 50;

// variables for stepper 2 (screw drive)
int inches = 1;
int rpi = 4; // 20 for the actual rod, 4 for testing
int spr2 = 200; // # steps to a single rotation
float maxSpeed2 = 800.0; // how fast can we run this thing?
float accel2 = 800.0;

// set up the servo object to manage the drill up/down action
Servo myservo;

bool isDrillDown = false;

bool isDecoratorRunning = false;
// Adafruit_MotorShield AFMSbot(0x61); // Rightmost jumper closed
Adafruit_MotorShield AFMStop(0x60); // Default address, no jumpers

// Connect two steppers with 200 steps per revolution (1.8 degree)
// to the top shield
Adafruit_StepperMotor *myStepper1 = AFMStop.getStepper(200, 1);
Adafruit_StepperMotor *myStepper2 = AFMStop.getStepper(200, 2);

Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();
// Now we’ll wrap the 2 steppers in an AccelStepper object
AccelStepper stepper1(forwardstep1, backwardstep1);
AccelStepper stepper2(forwardstep2, backwardstep2);

String otxt;
String otxt2;

void configSineWave() {

// variables for stepper 1 (mandrel)
circ = 8;
spr1 = 200;
amplitude = 50;
maxSpeed1 = 200.0;
accel1 = 100.0; // accelerate/decelerate softens end points, creating roundover effect

// variables for stepper 2 (screw drive)
inches = 1;
rpi = 4; // 20 for the actual rod, 4 for testing
spr2 = 200; // # steps to a single rotation
maxSpeed2 = 800.0; // how fast can we run this thing?
accel2 = 800.0;
}

void configSpiralLeft() {

// variables for stepper 1 (mandrel)
circ = 800 * inches; // should be a calculated value number of revolutions at maxspeed for screw drive to move those inches
spr1 = 200;
maxSpeed1 = 100.0;
amplitude = -1000000; // near infinite
accel1 = 100.0; // accelerate/decelerate softens end points, creating roundover effect

// variables for stepper 2 (screw drive)
inches = 1;
rpi = 4; // 20 for the actual rod, 4 for testing
spr2 = 200; // # steps to a single rotation
maxSpeed2 = 800.0; // how fast can we run this thing?
accel2 = 800.0;
}

void configSpiralRight() {

// variables for stepper 1 (mandrel)
circ = 800 * inches; // should be a calculated value number of revolutions at maxspeed for screw drive to move those inches
spr1 = 200;
maxSpeed1 = 100.0;
amplitude = 1000000; // near infinite
accel1 = 100.0; // accelerate/decelerate softens end points, creating roundover effect

// variables for stepper 2 (screw drive)
inches = 1;
rpi = 4; // 20 for the actual rod, 4 for testing
spr2 = 200; // # steps to a single rotation
maxSpeed2 = 800.0; // how fast can we run this thing?
accel2 = 800.0;
}

void lcdMsg(int line, String txt) {
if (line == 1) {
if (txt == otxt) return;
otxt = txt;
}
else {
if (txt == otxt2) return;
otxt2 = txt;
}
lcd.setCursor(0,line);
lcd.print(” “);
lcd.setCursor(0,line);
lcd.print(txt);
}

void DrillUp() {
lcdMsg(1,”DRILL UP”);
myservo.write(DRILLUP);
isDrillDown = false;
}

void DrillDown() {
lcdMsg(1,”DRILL DOWN”);
myservo.write(DRILLDOWN);
isDrillDown = true;
}

void StopDrill() {

DrillUp();

lcdMsg(1,”Stop”);

stepper1.disableOutputs();
stepper2.disableOutputs();

isDecoratorRunning = false;
}

void RunDrill() {

configSineWave();

DrillDown();

lcdMsg(1,”Running”);

stepper1.enableOutputs();
stepper2.enableOutputs();

// Idle the mandrel rotation
stepper1.moveTo(amplitude);
stepper1.setMaxSpeed(maxSpeed1);
stepper1.setAcceleration(accel1);

// set the rewind in action
// stepper2.moveTo(1 * inches * rpi * spr2);
stepper2.moveTo( RIGHT * abs(driveEndPos) );
stepper2.setMaxSpeed(maxSpeed2);
stepper2.setAcceleration(accel2);
isDecoratorRunning = true;
Serial.println(“Running RunDrill”);

}

void RewindDrill() {
lcdMsg(1,”Rewind”);
DrillUp();

lcdMsg(1,”Rewind”);

stepper1.enableOutputs();
stepper2.enableOutputs();

// Idle the mandrel rotation
stepper1.moveTo(stepper1.currentPosition());
stepper1.setMaxSpeed(0.0);
stepper1.setAcceleration(0.0);

// set the rewind in action
int target = stepper2.currentPosition()-driveStartPos;
Serial.print(“Rewind: stepper2.currentPosition() = “);
Serial.println(stepper2.currentPosition());
Serial.print(“Rewind: target = “);
Serial.println(target);
stepper2.moveTo( LEFT *abs(target) );
stepper2.setMaxSpeed(maxSpeed2);
stepper2.setAcceleration(accel2);

// If we are up against the switch, we need to rewind away from the switch to
// TODO: figure a way to “clear” ourselves from the microswitches at either end
aw = digitalRead(AWAYPIN);
while (!aw) {
stepper2.run();
aw = digitalRead(AWAYPIN);
}

isDecoratorRunning = true;
Serial.println(“Running Rewind”);

}

uint8_t obuttons;

void checkButtons() {

uint8_t buttons = lcd.readButtons();

if (buttons) {

if (buttons & BUTTON_UP) {
lcd.setBacklight(WHITE);
DrillUp();
}

if (buttons & BUTTON_DOWN) {
lcd.setBacklight(WHITE);
DrillDown();
}

if (buttons & BUTTON_LEFT) {
lcd.setBacklight(WHITE);
RunDrill();
}

if (buttons & BUTTON_RIGHT) {
lcd.setBacklight(WHITE);
RewindDrill();
}

if (buttons & BUTTON_SELECT) {
lcdMsg(0,”STOP”);
lcdMsg(1,””);
delay(1000);
lcdMsg(0,”Ready”);
lcd.setBacklight(OFF);
DrillUp();
isDecoratorRunning = false;
}

obuttons = buttons;

}
}

// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
// wrappers for the first motor!
void forwardstep1() {
myStepper1->onestep(FORWARD, SINGLE);
}
void backwardstep1() {
myStepper1->onestep(BACKWARD, SINGLE);
}
// wrappers for the second motor!
void forwardstep2() {
myStepper2->onestep(FORWARD, SINGLE);
}
void backwardstep2() {
myStepper2->onestep(BACKWARD, SINGLE);
}

void initializePosition() {
//
// TODO: change algorithm to look for the start, and just reset the start to 0 and return.

lcdMsg(0, “Find start…”);
lcdMsg(1,””);

// start looking for the start position
hm = digitalRead(HOMEPIN);
aw = digitalRead(AWAYPIN);
if (hm == 0) {
// I’m already up against the start position, move away until the switch clears
Serial.println(“up against start pos”);
stepper2.moveTo(100000);
stepper2.setMaxSpeed(maxSpeed2);
stepper2.setAcceleration(accel2);

if (!digitalRead(HOMEPIN))
Serial.println(“clearing start position”);

while (!digitalRead(HOMEPIN)) {
stepper2.run();

if (!digitalRead(AWAYPIN)) { // We hit the end position, before finding the start position switch open
// check wiring, switch contacts, and switch trigger wire for interference
lcdMsg(1, “ERR 1A”);
lcdMsg(0, “init pos fail”);
delay(120000); // two minutes
}
}
Serial.println(“cleared start position”);
}

// begin rewinding until we hit the home position switch
stepper2.moveTo(-1000000);
Serial.println(“check for start position”);
while (hm = digitalRead(HOMEPIN)) {
stepper2.run();
}
stepper2.setCurrentPosition(0); // set to 0 as our reference position
driveStartPos = stepper2.currentPosition()+EXTENT_OFFSET;

Serial.print( “driveStartPos = “);
Serial.println( driveStartPos );

lcdMsg(0, “Find end…”);
// Now repeat the process looking for the end position
Serial.println(“check for end position”);

stepper2.moveTo(1000000);
while (digitalRead(AWAYPIN))
stepper2.run();

driveEndPos = stepper2.currentPosition()-EXTENT_OFFSET;

// // for now, we start with assumption mandrel is in the start position
stepper1.setCurrentPosition(0);
mandrelZeroPos = stepper1.currentPosition();

RewindDrill();

Serial.print( “driveStartPos = “);
Serial.println( driveStartPos );
Serial.print(“driveEndPos = “);
Serial.println (driveEndPos);
Serial.print(“stepper2.currentPosition() = “);
Serial.println (stepper2.currentPosition());
Serial.print(“mandrelZeroPos = “);
Serial.println(mandrelZeroPos);

}
void setup()
{
Serial.begin(9600);
lcd.begin(16,2);
lcd.clear();
lcd.setBacklight(WHITE);

// initialize pins for reading the boundaries
pinMode(HOMEPIN, INPUT);
pinMode(AWAYPIN, INPUT);
digitalWrite(HOMEPIN, HIGH); // activate the pull-up resistors to make sure we’re not reading
digitalWrite(AWAYPIN, HIGH); // a floating value from the inputs when the switch is op

AFMStop.begin(); // Start the motor shield

stepper1.moveTo(0);
stepper1.setMaxSpeed(0.0);
stepper1.setAcceleration(200.0);

stepper2.moveTo(0);
stepper2.setMaxSpeed(800.0);
stepper2.setAcceleration(800.0);
myservo.attach(SERVO_PIN); // attaches the servo on pin 10 to the servo object – raise and lower the drill arm

Serial.println(“servo = ” + myservo.read());
if (myservo.read() != DRILLUP) {
myservo.write(DRILLUP); // start with the drill in the up position
}
isDrillDown = false;

// make sure the drill platform is in the up position before seeking the boundaries
initializePosition();

lcdMsg(0,”Ready”);

}

void loop()
{

hm = digitalRead(HOMEPIN);
aw = digitalRead(AWAYPIN);
if ((hm+aw)<2) {
StopDrill();
lcdMsg(0,”Boundary switch”);
lcdMsg(1,”trigger – ABEND”);
isDecoratorRunning = false;
Serial.print (“hm = “);
Serial.print (digitalRead(HOMEPIN));
Serial.print (” aw = “);
Serial.println (digitalRead(AWAYPIN));
Serial.print( “driveStartPos = “);
Serial.println( driveStartPos );
Serial.print(“driveEndPos = “);
Serial.println (driveEndPos);
Serial.print(“stepper2.currentPosition() = “);
Serial.println (stepper2.currentPosition());
Serial.print(“mandrelZeroPos = “);
Serial.println(mandrelZeroPos);

delay(5000);
}
if (isDecoratorRunning) {
stepper1.run();
stepper2.run();
// Change mandrel direction at the limits
if (stepper1.distanceToGo() == 0)
stepper1.moveTo(-stepper1.currentPosition());

if (stepper2.distanceToGo() == 0)
isDecoratorRunning = false;
}
else {
lcdMsg(0,”Ready”);
lcdMsg(1,””);
}

checkButtons();

}