Future City Workshop Project "UNDERGO". Copyright © 2015 by team "UnderGo".

undergoSystem.ino

//===================================Under Ground Carge System===================================
/*
  The system is based on Arduino UNO and modules including step motor, DC motors, etc.
  Connect 4 switch for mode switch on pin 0, 1, 2, 3; 1 step motor for platform on pin 4, 5, 6, 7;
  Connect 3 servo motors for gate on pin 9, 10, 11; 3 output for DC motors on pin A0, A1, A2.
  Function "setMotor" setup the gates status.
  Function "setOutput" setup the DC motors status.
*/
//================================================================================================

#include <Servo.h>
#include <Stepper.h>
Stepper stepper(16, 7, 6, 5, 4);
Servo servoMain1;              //pin 9
Servo servoMain2;              //pin 10
Servo servoMain3;              //pin 11

void setup(){
  //initialize.
  servoMain1.attach(9);      //A.
  servoMain2.attach(10);     //B.
  servoMain3.attach(11);     //reserve & takeout.
  setMotor(170, 170, 0);
  stepper.setSpeed(900);
  
  //mode switch.
  pinMode(0, INPUT);          //LOW for A.
  pinMode(1, INPUT);          //LOW for B.
  pinMode(2, INPUT);          //LOW for reserve.
  pinMode(3, INPUT);          //LOW for takeout.
  digitalWrite(0, HIGH);
  digitalWrite(1, HIGH);
  digitalWrite(2, HIGH);
  digitalWrite(3, HIGH);
  
  //hardware output.
  pinMode(A0, OUTPUT);          //A tunnel motor.
  pinMode(A1, OUTPUT);          //B tunnel motor.
  pinMode(A2, OUTPUT);          //takeout motor.
  setOutput(0, 0, 0);
}

void loop(){
  if(!digitalRead(0)){                    //A tunnel mode.
    setMotor(90, 180, 0);
    setOutput(1, 0, 0);
  }
  else if(!digitalRead(1)){               //reserve mode.
    setMotor(180, 180, 90);
    setOutput(0, 0, 0);
  }
  else if(!digitalRead(2)){               //takeout to tunnel B mode.
    setMotor(180, 180, 90);
    setOutput(0, 0, 1);
    delay(1000);
    setMotor(180, 90, 0);
    setOutput(0, 1, 0);
  }
  else{                                   //waiting mode.
    setMotor(180, 180, 0);
    setOutput(0, 0, 0);
  }
  stepper.step(512);
}

void setMotor(int r, int a, int b){      //A, B, reserve & takeout gate.
  servoMain1.write(r);
  servoMain2.write(a);
  servoMain3.write(b);
}

void setOutput(int O0, int O1, int O2){      //A, B, takeout motor.
  digitalWrite(A0, O0);
  digitalWrite(A1, O1);
  digitalWrite(A2, O2);
}