Arduino Code

//Alexander Hale

//5/28/2017

//Robotics 421

#include <Stepper.h>

#include <Servo.h>

//misc stuff

int roboState = 7, dcBDelay = 300, stepBSteps = 200, totMoves = 5;

steps = 200;

Servo servoNet; //attach to pin 11

rpms = 300;

//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 = 10,

opTurnSig = 12, returnSig = 13;

//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

  servo.attach(11);

  pinMode(blockSensor, INPUT);

  pinMode(forwardSensor, INPUT);

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

  pinMode(opTurnSig, INPUT);

  pinMode(returnSig, OUTPUT);

  pinMode(enableLift, OUTPUT);

  pinMode(enableUnused, OUTPUT);

  pinMode(enableWheels, OUTPUT);

  pinMode(enableClawDC, OUTPUT);

}

int blockTest(){

  //returns 1 if block is present

  return(!digitalRead(blockSensor));

}

void loop() {

    if((roboState == 7) && digitalRead(opTurnSig){

      roboState = 1;

    }

    switch (roboState){

      case 1: //Approach Tower

        digitalWrite(returnSig = LOW); //turn off our signal to opponent

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

          digitalWrite(enableWheels, HIGH);

          //dc motors move forward

          digitalWrite(DCm1, HIGH);

          digitalWrite(DCm2, LOW);

          digitalWrite(DCm3, HIGH);

          digitalWrite(DCm4, LOW);

        }

        else{ //robot has reached tower, stop

          digitalWrite(enableWheels, LOW);

          //dc motors stop

          digitalWrite(DCm1, LOW);

          digitalWrite(DCm2, LOW);

          digitalWrite(DCm3, LOW);

          digitalWrite(DCm4, LOW);

          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

        digitalWrite(enableWheels, HIGH);

        //dc motors move forward

        digitalWrite(DCm1, LOW);

        digitalWrite(DCm2, HIGH);

        digitalWrite(DCm3, LOW);

        digitalWrite(DCm4, HIGH);

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

        digitalWrite(enableWheels, LOW);

        //dc motors stop

        digitalWrite(DCm1, LOW);

        digitalWrite(DCm2, LOW);

        digitalWrite(DCm3, LOW);

        digitalWrite(DCm4, LOW);

        roboState = 8; //next step: move down

        break;

      case 3://Clamp Claw

        digitalWrite(enableClawDC, HIGH);

        //turn dc claw motor

        digitalWrite(DCm1, HIGH);

        digitalWrite(DCm2, LOW);

        delay(4000); //wait while claw closes

        digitalWrite(enableClawDC, LOW); //turn off

        digitalWrite(DCm1, LOW);

        digitalWrite(DCm2, LOW);   

        /*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

         */

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

        break;

      case 4: //move forward for one block

        digitalWrite(enableWheels, HIGH);

        //dc motors move forward

        digitalWrite(DCm1, HIGH);

        digitalWrite(DCm2, LOW);

        digitalWrite(DCm3, HIGH);

        digitalWrite(DCm4, LOW);

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

        digitalWrite(enableWheels, LOW);

        //dc motors stop

        digitalWrite(DCm1, LOW);

        digitalWrite(DCm2, LOW);

        digitalWrite(DCm3, LOW);

        digitalWrite(DCm4, LOW);

        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

          digitalWrite(enableWheels, HIGH);

          //dc motors move back

          digitalWrite(DCm1, LOW);

          digitalWrite(DCm2, HIGH);

          digitalWrite(DCm3, LOW);

          digitalWrite(DCm4, HIGH);

        }

        else{ //robot has reached the back, stop

          digitalWrite(enableWheels, LOW);

          //dc motors stop

          digitalWrite(DCm1, LOW);

          digitalWrite(DCm2, LOW);

          digitalWrite(DCm3, LOW);

          digitalWrite(DCm4, LOW);

          //swing net in to catch block

          servoNet.write(180);

          delay(2000); //wait for servo

          roboState = 6; //next step: unclamp claw

        }

        break;

    case 6: //unclamp claw

      digitalWrite(enableClawDC, HIGH);

      //turn dc claw motor

      digitalWrite(DCm1, LOW);

      digitalWrite(DCm2, HIGH);

      delay(2000); //wait while claw opens

      //TODO: check time above.

      digitalWrite(enableClawDC, LOW); //turn off

      digitalWrite(DCm1, LOW);

      digitalWrite(DCm2, LOW);   

      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(0);

      digitalWrite(returnSig = HIGH); //signal opponent turn

      //flash some lights maybe?

      break;

    case 8: //move down

      if(totMoves == 0){

        roboState = 7; //no more moves, end turn

        break;

      }

      totMoves -= 1;

      digitalWrite(enableLift, HIGH);

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

      digitalWrite(enableLift, LOW);

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

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

      }

      break;

    case 9: //move up

      //while(unknown pin)

      digitalWrite(enableLift, HIGH);

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

      digitalWrite(enableLift, LOW);

      break;

    default:

      //disable all motors, reest servo

      digitalWrite(enableWheels, LOW);

      digitalWrite(enableUnused, LOW);

      digitalWrite(enableLift, LOW);

      digitalWrite(enableClawDC, LOW);

      servoNet.write(0);

      digitalWrite(returnSig = LOW);

      break;

    }//end roboState

}//end main loop