servo_dick_about_1.sketch

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
int pos = 0;    // variable to store the servo position


String inputString = "";         // a String to hold incoming data
boolean stringComplete = false;  // whether the string is complete


int home_switch_pin = 12;
int disc_switch_pin = 13;


void setup() {
  // initialize serial:
  Serial.begin(9600);
  // reserve 200 bytes for the inputString:
  inputString.reserve(200);

  myservo.attach(2);



  /* switches */
  /* INPUT_PULLUP enables the Arduino Internal Pull-Up Resistor */
  pinMode(home_switch_pin, INPUT_PULLUP);      // toolhead home switch
  pinMode(disc_switch_pin, INPUT_PULLUP); // have we landed on a disc?


}

void loop() {



  // print the string when a newline arrives:
  if (stringComplete) {
    Serial.println(inputString);

    if (inputString == "toolhead_status\n")
    {

      toolhead_state(); // status of toolhead @todo this is just a demo

    }

    if (inputString == "disc_status\n")
    {

      toolhead_contact_disc_state(); // status of toolhead @todo this is just a demo

    }

    if (inputString == "raise\n")
    {
      toolHeadRaise();
    }

    if (inputString == "lower\n")
    {
      toolHeadLower();
    }


    if (inputString == "test\n")
    {
      // picks a disk, puts it in the drive, takes disk
      // puts it into outtray, then travels away, comes back
      // gets disk from outtray, puts it in the drive again
      // picks it up again and pops it back in intray where it started

      goToIntray();
      pickDisk(); // down, grab, back-up
      goToDrive();
      dropDisk(); // down, release, back-up


      toolhead_state(); // status of toolhead @todo this is just a demo

      /*
        pickDisk();
        goToOuttray();
        dropDisk();
        goToIntray();
        goToDrive();
        goToOuttray();
        pickDisk();
        goToDrive();
        dropDisk();
        pickDisk();
        goToIntray();
        dropDisk();
      */
    }

    if (inputString == "pickDiskFromIntrayDeliverToDrive\n")
    {


      myservo.write(pos);


      goToIntray(); // move to right place
      pickDisk(); // down, grab, back-up
      goToDrive();
      dropDisk();
    }

    if (inputString == "pickDiskFromDriveDeliverToOuttray\n")
    {
      goToDrive();
      pickDisk();
      goToOuttray();
      dropDisk(); // down, release, back-up
    }



    // clear the string:
    inputString = "";
    stringComplete = false;
  }
}

/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

void goToIntray() {
  // move far left to in tray position
}
void goToDrive() {
  // go to the dvd drive (middle)
}
void goToOuttray() {
  // go far right for out tray (ripped pile)
}

void pickDisk() {
  // send down the toolhead, grab the disk, send toolhead home
  toolHeadLower();


  // pick up a disk


  toolHeadRaise();

}
void dropDisk() {

  // sends down the toolhead, drops the disk, send toolhead home
  toolHeadLower();


  // drop it


  toolHeadRaise();

}

bool toolHeadRaise() {
  // aka, 'go home toolhead' !
  Serial.println("100,raising tool head");

  // initial check
  if (!toolhead_state())
  {

    for (pos = 185; pos >= 0; pos -= 1) {
      // per degree check if home
      if (!toolhead_state())
      {
        // in steps of 1 degree
        myservo.write(pos);              // tell servo to go to position in variable 'pos'
        delay(10);                       // waits 15ms for the servo to reach the position
      }
    }

  }



  Serial.println("101,raised tool head");

  return true;
}

bool toolHeadLower() {
  Serial.println("102,lowering tool head");
  if (!toolhead_contact_disc_state())
  {
     for (pos = 0; pos <= 150; pos += 1) { // goes from 0 degrees to 180 degrees
      if (!toolhead_contact_disc_state())
      {
          // in steps of 1 degree
           myservo.write(pos);              // tell servo to go to position in variable 'pos'
          delay(15);                       // waits 15ms for the servo to reach the position
      }
    }
  }
 
  Serial.println("103,lowered tool head");
  return true;
}

bool toolhead_state() {
  // check status of switches
  bool toolhead_is_home = digitalRead(home_switch_pin);

  // switched the logic, due to pull up internal resistor
  if (!toolhead_is_home) {
    Serial.println("104,toolhead is home");
    return true;
  } else {
    Serial.println("105,toolhead is away on duty");
    return false;
  }
}

bool toolhead_contact_disc_state() {
  // check status of switches
  bool toolhead_contacted_disc = digitalRead(disc_switch_pin); // @todo disc_switch_pin

  // switched the logic, due to pull up internal resistor
  if (!toolhead_contacted_disc) {
    Serial.println("106,disc not present");
    return true;
  } else {
    Serial.println("107,disc present");
    return false;
  }
}