gistfile1.txt

#include <AFMotor.h>
#include <ServoTimer1.h>
            
//#define DEBUG 1

int analogPin = 0;
int sensorPin = 2;
int servo1Pin = 9; 

AF_Stepper motorX(48, 2);
AF_Stepper motorY(48, 1);
ServoTimer1 servo1;

const int SERVO_UP = 1;
const int SERVO_DOWN = -1;
const boolean TOUCHING = HIGH;
const boolean NOT_TOUCHING = LOW;

int restPos = 0;
int minDepth = 160;
int maxDepth = 40;
int surface = restPos;
int stepperDirection = FORWARD;
int stepDelay = 10;
int vtouch = 790; 

int currentDrillDepth;
int surfaceHover = 20;
int serialReadNum;

void setup() 
{ 
  //digitalWrite(sensorPin, LOW);
  //pinMode(sensorPin, INPUT); 
  Serial.begin(19200); 
  servo1.attach(servo1Pin);
  servo1.write(minDepth);
  delay(1000);
} 
void initMotors()
{ 
  servo1.attach(servo1Pin);
  motorX.setSpeed(100);  // 100 rpm 
  motorY.setSpeed(100);  // 100 rpm  
  currentDrillDepth = minDepth;  
  servo1.write(minDepth);
  delay(1000);
  turnSteppersOn(); 
}
void turnSteppersOn()
{ 
  motorX.step(1, FORWARD, MICROSTEP); // SINGLE, DOUBLE, INTERLEAVE
  motorX.step(1, BACKWARD, MICROSTEP); 
  motorX.release();
  motorY.step(1, FORWARD, MICROSTEP); 
  motorY.step(1, BACKWARD, MICROSTEP); 
  motorY.release();
  delay(1000);
}
void moveMotorX() 
{     
  int count = (serialReadNum == 0) ? 1 : serialReadNum;
  motorX.step(count, stepperDirection, MICROSTEP); 
  Serial.print("MotorMovedLR "
  );Serial.println(count); 
}
void moveMotorY() 
{     
  int count =  (serialReadNum == 0) ? 1 : serialReadNum;
  motorY.step(count, stepperDirection, MICROSTEP); 
  Serial.print("MotorMovedFB ");Serial.println(count); 
}

void probeDown()
{
  while(analogRead(analogPin) < vtouch)
  {
    currentDrillDepth += SERVO_DOWN;
    servo1.write(currentDrillDepth);
    delay(stepDelay);
    if(currentDrillDepth <= maxDepth)
    {
      Serial.print("DrillMax ");Serial.println(maxDepth); 
      break;
    }
  }
  surface = currentDrillDepth;
  Serial.println("");Serial.print("Probe Surface ");Serial.println(surface);
  hover();
}


void drillDown(int depth)
{
  while(analogRead(analogPin) < vtouch)
  {
    currentDrillDepth += SERVO_DOWN;
    servo1.write(currentDrillDepth);
    delay(stepDelay);
    if(currentDrillDepth <= maxDepth)
    {
      Serial.print("DrillMax ");Serial.println(maxDepth); 
      break;
    }
  }
  surface = currentDrillDepth;
  Serial.print("Drill Surface ");Serial.println(surface);
  servo1.write(surface + depth * SERVO_DOWN);
  delay(depth * stepDelay + surfaceHover * stepDelay); 
  hover();  
}

void hover()
{
  while(analogRead(analogPin) > vtouch)
  {
    currentDrillDepth += SERVO_UP;
    servo1.write(currentDrillDepth);
    delay(stepDelay);
    if(currentDrillDepth >= minDepth)
    {
      Serial.print("DrillMax ");Serial.println(maxDepth); 
      //probeDown();
      break;
    }
  } 
  surface = currentDrillDepth;
  Serial.print("Hover Surface ");Serial.println(surface);
  currentDrillDepth = surface + surfaceHover * SERVO_UP;
  servo1.write(currentDrillDepth);
  delay(surfaceHover * stepDelay);
  Serial.println("DrillTop ");
}

void loop() 
{ 
  if (Serial.available()) 
  {
    char ch = Serial.read();
    
    switch(ch) {
      case '0'...'9':
        serialReadNum = serialReadNum * 10 + ch - '0';
        break;
      case 's':
        servo1.write(serialReadNum);
        #ifdef DEBUG
          Serial.print(serialReadNum);
        #endif
        serialReadNum = 0;
        break;
      case 'f':
        stepperDirection = FORWARD;
        moveMotorY();
        serialReadNum = 0;
        break;
      case 'b':
        stepperDirection = BACKWARD;
        moveMotorY();
        serialReadNum = 0;
        break;
      case 'l':
        stepperDirection = BACKWARD;
        moveMotorX();
        serialReadNum = 0;
        break;
      case 'r':
        stepperDirection = FORWARD;
        moveMotorX();
        serialReadNum = 0;
        break;
      case 'p': 
        initMotors();
        currentDrillDepth = minDepth;
        probeDown(); 
        break;
      case 'o':
        serialReadNum = 0;
        currentDrillDepth = minDepth;
        servo1.write(minDepth);
        delay(500);
        servo1.detach();
        #ifdef DEBUG
          Serial.println("Turned Off");
        #endif
        break;
      case 'd':
        drillDown(serialReadNum);
        serialReadNum = 0;          
        break;
      case 'w':
        delay(serialReadNum);
        serialReadNum = 0;
        break;
      case 'a':
	#ifdef DEBUG
	  Serial.println("attach motors ");
	#endif   
	Serial.println("DrillTop");
        //initMotors();
        break;
      case 'i':
        int angle = servo1.read();
        //volatile boolean probeSensor = digitalRead(sensorPin);
	Serial.println("*** INFORMATION ***");
	//Serial.print("probeSensor :");Serial.println(probeSensor==TOUCHING ? "touching" : "not touching");
	Serial.print("probeSensor :");Serial.println(analogRead(analogPin));
	Serial.print("servo angle :");Serial.println(angle);
	Serial.print("currentDrillDepth :");Serial.println(currentDrillDepth);
        break;
    }
  } 
  //AF_Stepper::refresh();
}