Lego_Golf_v1_Arduino

#include <AFMotor.h> // Enables the Motor library

// Basic setup
AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
int ballSensor = A0;
int scoopSensor = A1;

int isRunning = 0;
int isReseting = 0;
int isBallReady = 0;
int isScoopReady = 0;

int readyPin = 13;
int busyPin = 12;

void setup() {
  Serial.begin(9600); // Enables Serial monitor for debugging purposes
  Serial.println("Serial test!"); // Test the Serial communication
  motor1.setSpeed(250);
  pinMode(readyPin, OUTPUT);
  pinMode(busyPin, OUTPUT);
}

void loop() {
    //Serial.println("Loop starting!");
    //delay(500);
    int ballSensorReading = analogRead(ballSensor);
    Serial.println(ballSensorReading);
    int ballSensorRational = map(ballSensorReading, 220, 320, 100, 0);
    Serial.println(ballSensorRational);
    if (ballSensorRational > 20) {
      isBallReady = 1;
    } else {
      isBallReady = 0;
    }

    int scoopSensorReading = analogRead(scoopSensor);
    Serial.println(scoopSensorReading);
    int scoopSensorRational = map(scoopSensorReading, 220, 350, 100, 0);
    Serial.println(scoopSensorRational);
    if (scoopSensorRational > 10) {
      isScoopReady = 1;
    } else {
      isScoopReady = 0;
    }
    
    if (isRunning == 0 && isReseting == 0 && isBallReady == 0 && isScoopReady == 0) {
      //do nothing
    } else if (isRunning == 0 && isReseting == 0 && isBallReady == 1 && isScoopReady == 0) {
      //do nothing  
    } else if (isRunning == 0 && isReseting == 0 && isBallReady == 1 && isScoopReady == 1) {
      isRunning = 1;
      motor1.run(BACKWARD);
      Serial.println("RAISE");
      digitalWrite(readyPin, LOW);
      digitalWrite(busyPin, HIGH);      
    } else if (isRunning == 1 && isReseting == 0 && isBallReady == 1 && isScoopReady == 0) {
      //no change
    } else if (isRunning == 1 && isReseting == 0 && isBallReady == 0 && isScoopReady == 0) {
      isRunning = 0;
      isReseting = 1;
      delay(500);
      motor1.run(FORWARD);
      Serial.println("RESET");
    } else if (isRunning == 0 && isReseting == 1 && isBallReady == 0 && isScoopReady == 0) {
      //no change
    } else if (isRunning == 0 && isReseting == 1 && isBallReady == 0 && isScoopReady == 1) {
      isRunning = 0;
      isReseting = 0;
      motor1.run(RELEASE);
      Serial.println("STOP");
      digitalWrite(readyPin, HIGH);
      digitalWrite(busyPin, LOW);
    } else if (isRunning == 0 && isReseting == 0 && isBallReady == 0 && isScoopReady == 1) {
      //do nothing
    }
    
    /*
    motor1.run(BACKWARD);
    delay(4000);
    motor1.run(RELEASE);
    delay(500);
    motor1.run(FORWARD);
    delay(2400);
    motor1.run(RELEASE);
    Serial.println("finish!");
    delay(5000);
    */
    

}