Fles Drive v2

#include <IRremote.h>
#include <Servo.h>

#define FILL_BOTTLE    0xa91
#define BOTTLE_FILLED  0xa92

// Arduino pins for the shift register
#define MOTORLATCH   12
#define MOTORCLK     4
#define MOTORENABLE  7
#define MOTORDATA    8

#define MOTOR1_A  2
#define MOTOR1_B  3
#define MOTOR2_A  1
#define MOTOR2_B  4
#define MOTOR3_A  5
#define MOTOR3_B  7
#define MOTOR4_A  0
#define MOTOR4_B  6

// Arduino pins for the PWM signals.
#define MOTOR1_PWM  11
#define MOTOR2_PWM  3
#define MOTOR3_PWM  6
#define MOTOR4_PWM  5
#define SERVO1_PWM  10
#define SERVO2_PWM  9

// Codes for the motor function.
#define FORWARD   1
#define BACKWARD  2
#define BRAKE     3
#define RELEASE   4

#define scanBlack           500
#define DRIVE_FULL_SPEED    255
#define DRIVE_MEDIUM_SPEED  150
#define DRIVE_SLOW_SPEED    125
#define DRIVE_STOP          0




// line scanners
const int scanPinMid   = 3;
const int scanPinLeft  = 5;
const int scanPinRight = 4;

// motor aandrijf pin
const int motorPin = 3;

// mosture sensor
const int mosturePin = 0;

// servo
Servo servo;

// infrarood
IRsend ir;

// time
unsigned long prevTimeAllFalse = 0;
unsigned long startedFillingBottleTime = 0;
unsigned long prevTimeTurning = 0;

bool isAllFalse = false;
bool isBottleEmpty = false;
bool isStopped = false;


const int STATE_DRIVING_TO_REFILL = 0;
const int STATE_DRIVING_TO_USER   = 1;
const int STATE_HOLDING_AT_USER   = 2;
const int STATE_HOLDING_AT_REFILL = 3;

int drivingState = 0;
int prevDrivingState = 0;




void setup()
{
  for (int i = 0; i < 5; i++) {
    ir.sendSony(FILL_BOTTLE, 12);
    delay(10);
  }

  Serial.begin(9600);

  // power supply
  pinMode(scanPinMid, INPUT);
  pinMode(scanPinLeft, INPUT);
  pinMode(scanPinRight, INPUT);

  // servo position
  servo.attach(SERVO1_PWM);

  // zet recht
  while (servo.read() != 90) {
    servo.write(90);
  }

  drivingState = STATE_DRIVING_TO_USER;
  prevDrivingState = drivingState;

  // TEST
  // isBottleEmpty = true;

  Serial.println("------------------------");
  Serial.println("Starting car.");
}



void drive(int speed, int angle = 90) {
  servo.write(angle);
  delay(100);
  motor(1, FORWARD, speed);
}



void loop()
{
  int scanMid = analogRead(scanPinMid);
  int scanLeft = analogRead(scanPinLeft);
  int scanRight = analogRead(scanPinRight);

  int mostureValue = analogRead(mosturePin);

  unsigned long curTime = millis();



  if (isStopped) {

    // als we bij gebruiker wachten, check fles mosture values
    if (mostureValue > 500) {
      isBottleEmpty = true;
      isStopped = false;

      prevDrivingState = drivingState;
      drivingState = STATE_DRIVING_TO_REFILL;

      drive(DRIVE_SLOW_SPEED);
    }



    // als fles leeg is, en we staan stil na ons ritje naar fill locatie, stuur signaal om fles te vullen
    if (drivingState == STATE_DRIVING_TO_REFILL) {
      for (int i = 0; i < 5; i++) {
        ir.sendSony(FILL_BOTTLE, 12);
        delay(10);
      }

      prevDrivingState = drivingState;
      drivingState = STATE_HOLDING_AT_REFILL;

      startedFillingBottleTime = curTime;
    }


    // fles is gevuld na 5 seconden. Geef dan seintje dat we weer kunnen rijden naar de gebruiker
    if (drivingState == STATE_HOLDING_AT_REFILL && curTime - startedFillingBottleTime > 5000) {
      isStopped = false;
      isBottleEmpty = false;

      prevDrivingState = drivingState;
      drivingState = STATE_DRIVING_TO_USER;

      drive(DRIVE_SLOW_SPEED);
    }

    return;

  } /* else {

    // houdt de motor aan als onze state dat wil
    if (drivingState == STATE_DRIVING_TO_USER || drivingState == STATE_DRIVING_TO_REFILL) {
      motor(1, FORWARD, DRIVE_SLOW_SPEED);
    }
  } */



  // rijdt vooruit tot we een zwarte lijn zien
  if (drivingState == STATE_DRIVING_TO_USER) {
    if (scanMid >= scanBlack || scanLeft >= scanBlack || scanRight >= scanBlack) {
      isReturningToUser = false;
    }

    prevDrivingState = drivingState;
    drivingState = STATE_DRIVING_TO_USER;

    motor(1, FORWARD, DRIVE_SLOW_SPEED);
    return;
  }



  // kijk of middelste scanner iets ziet
  if (scanMid >= scanBlack) {

    /*

      CANCEL TURNING

                      */

  // zo niet, kijk naar de linker en rechter en draai tot de middelste aanstaat
  } else {

    // links en links & rechts
    if (scanLeft >= scanBlack || (scanLeft >= scanBlack && scanRight >= scanBlack)) {
      Serial.println("left");

      if (isAllFalse) isAllFalse = false;

      int servoAngle = servo.read();
      if (servoAngle > 50) {
        if (curTime - prevTimeTurning > 1) {
          servoAngle--;
          servo.write(servoAngle);

          prevTimeTurning = curTime;

          Serial.println("turn left " + String(servoAngle));
        }
      }
    }

    // alleen rechts
    if (scanRight >= scanBlack && scanLeft < scanBlack) {
      Serial.println("right");

      if (isAllFalse) isAllFalse = false;

      int servoAngle = servo.read();
      if (servoAngle < 130) {
        if (curTime - prevTimeTurning > 1) {
          servoAngle++;
          servo.write(servoAngle);

          prevTimeTurning = curTime;

          Serial.println("turn right " + String(servoAngle));
        }
      }
    }


    // alle scanners false, zet motor uit na een tijdje
    if (scanRight < scanBlack && scanLeft < scanBlack) {
       if(!isAllFalse) {
         isAllFalse = true;
         prevTimeAllFalse = curTime;
       }
    }
  }


  // CHECK OUR NEXT STEP
  /////

  /*
    STOP AND WAIT AT USER UNTIL BOTTLE IS EMPTY ENOUGH FOR REFILL
                                                                 */
  if (drivingState == STATE_DRIVING_TO_USER && isAllFalse && (curTime - prevTimeAllFalse) > 250) {
    isStopped = true;

    prevDrivingState = drivingState;
    drivingState = STATE_HOLDING_AT_USER;

    motor(1, RELEASE, DRIVE_STOP);
  }


  /*
    STOP AND WAIT FOR REFILL
                            */
  if (drivingState == STATE_DRIVING_TO_REFILL && isAllFalse && (curTime - prevTimeAllFalse) > 250) {
    isStopped = true;

    prevDrivingState = drivingState;
    drivingState = STATE_HOLDING_AT_REFILL;

    motor(1, RELEASE, DRIVE_STOP);
  }


  /*
    GET READY TO GO TO REFILL ZONE
                                  */
  // if (drivingState == STATE_HOLDING_AT_USER && isBottleEmpty && isAllFalse && (curTime - prevTimeAllFalse) > 250) {
  //   servo.write(90);
  //   motor(1, RELEASE, DRIVE_STOP);

  //   // startedFillingBottleTime = curTime;
  //   isReturningToRefill = true;
  // }
}




//////
// ----------------------------
//////


void motor(int nMotor, int command, int speed)
{
  int motorA, motorB;

  if (nMotor >= 1 && nMotor <= 4)
  {
    switch (nMotor)
    {
    case 1:
      motorA   = MOTOR1_A;
      motorB   = MOTOR1_B;
      break;
    case 2:
      motorA   = MOTOR2_A;
      motorB   = MOTOR2_B;
      break;
    case 3:
      motorA   = MOTOR3_A;
      motorB   = MOTOR3_B;
      break;
    case 4:
      motorA   = MOTOR4_A;
      motorB   = MOTOR4_B;
      break;
    default:
      break;
    }

    switch (command)
    {
    case FORWARD:
      motor_output (motorA, HIGH, speed);
      motor_output (motorB, LOW, -1);     // -1: no PWM set
      break;
    case BACKWARD:
      motor_output (motorA, LOW, speed);
      motor_output (motorB, HIGH, -1);    // -1: no PWM set
      break;
    case BRAKE:
      motor_output (motorA, LOW, 255); // 255: fully on.
      motor_output (motorB, LOW, -1);  // -1: no PWM set
      break;
    case RELEASE:
      motor_output (motorA, LOW, 0);  // 0: output floating.
      motor_output (motorB, LOW, -1); // -1: no PWM set
      break;
    default:
      break;
    }
  }
}


void motor_output (int output, int high_low, int speed)
{
  int motorPWM;

  switch (output)
  {
  case MOTOR1_A:
  case MOTOR1_B:
    motorPWM = MOTOR1_PWM;
    break;
  default:
    // Use speed as error flag, -3333 = invalid output.
    speed = -3333;
    break;
  }

  if (speed != -3333)
  {
    // Set the direction with the shift register
    // on the MotorShield, even if the speed = -1.
    // In that case the direction will be set, but
    // not the PWM.
    shiftWrite(output, high_low);

    // set PWM only if it is valid
    if (speed >= 0 && speed <= 255)
    {
      analogWrite(motorPWM, speed);
    }
  }
}


void shiftWrite(int output, int high_low)
{
  static int latch_copy;
  static int shift_register_initialized = false;

  // Do the initialization on the fly,
  // at the first time it is used.
  if (!shift_register_initialized)
  {
    // Set pins for shift register to output
    pinMode(MOTORLATCH, OUTPUT);
    pinMode(MOTORENABLE, OUTPUT);
    pinMode(MOTORDATA, OUTPUT);
    pinMode(MOTORCLK, OUTPUT);

    // Set pins for shift register to default value (low);
    digitalWrite(MOTORDATA, LOW);
    digitalWrite(MOTORLATCH, LOW);
    digitalWrite(MOTORCLK, LOW);
    // Enable the shift register, set Enable pin Low.
    digitalWrite(MOTORENABLE, LOW);

    // start with all outputs (of the shift register) low
    latch_copy = 0;

    shift_register_initialized = true;
  }

  // The defines HIGH and LOW are 1 and 0.
  // So this is valid.
  bitWrite(latch_copy, output, high_low);

  // Use the default Arduino 'shiftOut()' function to
  // shift the bits with the MOTORCLK as clock pulse.
  // The 74HC595 shiftregister wants the MSB first.
  // After that, generate a latch pulse with MOTORLATCH.
  shiftOut(MOTORDATA, MOTORCLK, MSBFIRST, latch_copy);
  delayMicroseconds(5);    // For safety, not really needed.
  digitalWrite(MOTORLATCH, HIGH);
  delayMicroseconds(5);    // For safety, not really needed.
  digitalWrite(MOTORLATCH, LOW);
}