Short answer is I used PaulS technique. It may not be the most effecient, but it seemed to get things up and going pretty quickly.
Here is the complete code dump for those that are interested.
//Command Reference
//M = Move to step i.e. M200 moves to step 200
//Z = Zeros the arm to -32000, arm will move up until it hits top
// light sensor
//P = Picks up disk by moving down until sensor on arm is tripped
//S = Displays the status os the sensors
//X = Siliniod Control i.e. X1 turns on the siliniod X0 turns it off
#include <Servo.h>
#define SOP '<'
#define EOP '>'
bool started = false;
bool ended = false;
Servo myservo;
const int headSensor = A1;
const int armSensor = A2;
const int DirPin = 3;
const int StepPin = 4;
const int silinoidPin = 8;
const int stepDuration = 50;
const int stepDelay = 100;
char inData[80];
byte index;
int currentStep = -32000;
void setup(){
pinMode(DirPin, OUTPUT);
pinMode(StepPin, OUTPUT);
Serial.begin(57600);
pinMode(silinoidPin, OUTPUT);
myservo.attach(7); //attaches the servo to pin 7
}
void loop() {
while (Serial.available() > 0) {
char inChar = Serial.read();
if (inChar == SOP)
{
index = 0;
inData[index] = '\0';
started = true;
ended = false;
}
else if (inChar == EOP)
{
ended = true;
break;
}
else
{
if (index < 79)
{
inData[index] = inChar;
index++;
inData[index] = '\0';
}
}
}
// We are here either because all pending serial
// data has been read OR because an end of
// packet marker arrived. Which is it?
if (started && ended)
{
//The End of Packet Market Arrived. Process the packet
char parrot = inData[0];
inData[0] = ' ';
char cmd = inData[1];
inData[1] = ' ';
int val = atoi(inData);
//This is where we decifer the commands coming from the pc
if (cmd == 'M')
{
//Serial.print("Move to: ");
// Serial.println(val);
moveTo(val);
Serial.print("Move Complete to: ");
Serial.print(val);
}
else if (cmd == 'S')
{
readSensor(val);
}
else if (cmd == 'Z')
{
zeroArm();
}
else if (cmd == 'P')
{
pickUpDisk();
}
else if (cmd == 'X')
{
silControl(val);
}
else if (cmd == 'E')
{
pushButton(val);
}
//Reset for the next packet
started = false;
ended = false;
index = 0;
inData[index] = '\0';
Serial.print("<");
Serial.print(parrot);
Serial.print(">");
}
Serial.flush();
}
//This is the function that moves the motor to a chosen point
void moveTo(int val)
{
while(currentStep > val)
{
//The step requested is lower than the current step we are currently on
takeStep(false);
currentStep--;
}
while(currentStep < val)
{
//The step requested is higher than the current step we are currently on
takeStep(true);
currentStep++;
}
}
void pickUpDisk()
{
boolean sensorTriggered = false;
int attemptsMade = 0;
while (sensorTriggered == false)
{
//for safety first we engage the silinoid
digitalWrite(silinoidPin, HIGH);
//we move the arm down until the head sensor makes contact
while (analogRead(headSensor) <800)
{
takeStep(true);
currentStep++;
}
int numberOfSteps = 500; //the number of steps to take after sensor triggered
for (int x = 0; x < numberOfSteps; x++)
{
takeStep(true);
currentStep++;
}
digitalWrite(silinoidPin, LOW);
delay(300);
//now we think we have the disk, we back off numble of steps + 100
//and check the sensor, and try again if the sensor is not triggered
for (int y = 0; y < (numberOfSteps + 200); y++)
{
takeStep(false);
currentStep--;
}
if (analogRead(headSensor) > 800)
{
sensorTriggered = true;
}
//this is here to limit the amount of attempts to 10 tries
if (attemptsMade > 10)
{
Serial.println("Could not Pick Up Disk");
break;
}
else
{
attemptsMade++;
numberOfSteps = (numberOfSteps + 20);
}
}