Arduino Code

//Alexander Hale

//5/31/2017

//Robotics 421

#include <Stepper.h>

#include <Servo.h>

//misc stuff

int roboState = 1, initial = 0, dcBDelay = 200, stepBSteps = 2000, debugTime = 300, totMoves = 0, maxMoves = 3;

int steps = 200, cOpen = 80, cClose = 5, nOpen = 10, nClose = 160;

Servo servoNet, servoClaw; //attach to pin 11 and 10

int rpms = 250;

//pin declarations

//controller enables

const int enableLift = 0, enableUnused = 1,

enableWheels = 2, enableClawDC = 3;

//wheel pins

const int DCm1 = 7, DCm2 = 6, DCm3 = 5, DCm4 = 4;

//sensor pins

const int blockSensor = 8, forwardSensor = 9, backSensor = 13,

opTurnSig = 12;

//stepper motor uses same pins as wheel DC motors

//be sure to disable and enable correct drivers

Stepper motor = Stepper(steps, 4, 5, 6, 7); 

void setup() {

  pinMode(LED_BUILTIN, OUTPUT);

  motor.setSpeed(rpms);

  //pin setup

  servoNet.attach(11);

  servoClaw.attach(10);

  pinMode(blockSensor, INPUT);

  pinMode(forwardSensor, INPUT);

  pinMode(backSensor, INPUT_PULLUP); //TODO: check this

  pinMode(opTurnSig, INPUT);

  pinMode(enableLift, OUTPUT);

  pinMode(enableUnused, OUTPUT);

  pinMode(enableWheels, OUTPUT);

  pinMode(enableClawDC, OUTPUT);

}

int blockTest(){

  //returns 1 if block is present

  delay(1000);

  return(!digitalRead(blockSensor));

}

void moveUp(){

  digitalWrite(enableLift, HIGH);

  motor.step(-stepBSteps); //move the claw up

  digitalWrite(enableLift, LOW);

}

void moveDown(){

  digitalWrite(enableLift, HIGH);

  motor.step(stepBSteps); //move the claw down

  digitalWrite(enableLift, LOW);

}

void wheelStop(){

  digitalWrite(enableWheels, LOW);

  //dc motors stop

  digitalWrite(DCm1, LOW);

  digitalWrite(DCm2, LOW);

  digitalWrite(DCm3, LOW);

  digitalWrite(DCm4, LOW);

}

void forward(){

  digitalWrite(enableWheels, HIGH);

  //dc motors move forward

  digitalWrite(DCm1, HIGH);

  digitalWrite(DCm2, LOW);

  digitalWrite(DCm3, HIGH);

  digitalWrite(DCm4, LOW);

}

void backward(){

  digitalWrite(enableWheels, HIGH);

  //dc motors move backward

  digitalWrite(DCm1, LOW);

  digitalWrite(DCm2, HIGH);

  digitalWrite(DCm3, LOW);

  digitalWrite(DCm4, HIGH);

}

void loop() {

  if(initial == 0){

      delay(1000);

      servoNet.write(nOpen);

      servoClaw.write(cOpen);

      digitalWrite(enableWheels, LOW);

      digitalWrite(enableUnused, LOW);

      digitalWrite(enableLift, LOW);

      digitalWrite(enableClawDC, LOW);

      initial = 1;

    }

  while(digitalRead(opTurnSig)){

    if(initial == 1){

      delay(1000);

      servoNet.write(nOpen);

      servoClaw.write(cOpen);

      initial = 2;

    }

    switch (roboState){

      case 1: //Approach Tower

        if(digitalRead(forwardSensor)){ //moves forward while wheel sensor is not tripped

          forward();

        }

        else{ //robot has reached tower, stop

          wheelStop();

          delay(debugTime);

          if(blockTest()){ //is block there?

            roboState = 4;//next step: move forward one block

          }

          else{

            roboState = 8; //next step: move down

          }

        }

        break;

      case 2: //move back one block

        backward();

        delay(dcBDelay); //wait for robot to move back

        wheelStop();

        delay(debugTime);

        roboState = 8; //next step: move down

        break;

      case 3://Clamp Claw

        servoClaw.write(cClose);

        delay(1000);

        delay(debugTime);

        roboState = 5; //next step: return with block

        break;

      case 4: //move forward for one block

        forward();

        delay(dcBDelay); //wait for robot to move forward

        wheelStop();

        delay(debugTime);

        if(blockTest()){ //is block there?

          roboState = 3; //next step: grab block

        }

        else{

          roboState = 2; //next step: move back one block

        }

        break;

    case 5: //return with block

        /*NOTE: to keep pressure on block, we could wire the DC motors such that the 

         * back commands went the same way as the clamp commands and enable both drivers

         * to simutaneously move back and keep pressure on the block

         */

      if(digitalRead(backSensor)){ //moves back while wheel sensor is not tripped

          backward(); 

        }

        else{ //robot has reached the back, stop

          wheelStop();

          delay(debugTime);

          for(int i = 0; i < totMoves; i++){

            moveUp();

          }

          delay(debugTime);

          servoNet.write(nClose);

          delay(1000); //wait for servo

          //swing net in to catch block

          delay(debugTime);

          roboState = 6; //next step: unclamp claw

        }

        break;

    case 6: //unclamp claw

      servoClaw.write(cOpen); //unclamp

      delay(1000);

      servoNet.write(nOpen); //move net back

      delay(1000);

      for(int i = 0; i < totMoves; i++){

            moveDown();

          }

      delay(1000);

      delay(debugTime);     

      roboState = 7; //next step: end turn

      break;

    case 7: //end turn

      //disable all motors, reest servo

      digitalWrite(enableWheels, LOW);

      digitalWrite(enableUnused, LOW);

      digitalWrite(enableLift, LOW);

      digitalWrite(enableClawDC, LOW);

      //servoNet.write(10);

      //flash some lights maybe?

      //add delay?

      break;

    case 8: //move down

      if(totMoves == maxMoves){

        roboState = 5; //no more moves, move back and end turn

        break;

      }

      totMoves += 1;

      moveDown();

      delay(debugTime);

      if(blockTest()){ //is block there?

        roboState = 4;//next step: move forward one block

      }

      break;

    default:

      //disable all motors, reset servo

      digitalWrite(enableWheels, LOW);

      digitalWrite(enableUnused, LOW);

      digitalWrite(enableLift, LOW);

      digitalWrite(enableClawDC, LOW);

      servoNet.write(nOpen);

      servoClaw.write(cOpen);

      break;

    }//end roboState

  }//end while loop

  roboState = 1;

}//end main loop