Träkker 08.02.2017-2

#include <Arduino.h>
// PIN CONSTANTS
const byte PIN_LED = 13;      // Pin 13 has an LED connected on most Arduino boards.
const byte PIN_LDR_WEST = A0;   // LDR lääne poole paneeli
const byte PIN_LDR_EAST = A1;   // LDR ida poole paneeli
const byte PIN_LIMIT_WEST = A2;   // lääne lõpu lüliti.Peab olema high lõpus low. Kui valgus langeb määratud tasemele-LDR_NIGHT siis pöörab itta?
const byte PIN_LIMIT_EAST = A3;   // ida lõpu lüliti.peab olema high.Lõpus low.Et ei hakkaks enam pöörama itta. Nii riist- kui tarkvaraliselt
const byte PIN_MOTOR_ENABLE = 10; // enable H-Bridge motor driver pin
const byte PIN_MOTOR_DRIVE_WEST = 7;// OUT 1 h-bridge
const byte PIN_MOTOR_DRIVE_EAST = 8;// OUT 2 h-bridge vahetada OUT 1-ga kui pöörab valele poole

// LOGIC CONSTANTS
const byte LDR_DEAD_BAND = 5;    // erinevus LDR-de vahel, et hakkaks pöörama
const byte LDR_NIGHT = 1;      // väärtus, et on pime ja paneel sõidaks itta ?
const int LDR_INTERVAL = 100;   // check LDR every x milliseconds
const int MOTOR_STOP = 0;     // 0 to motor_control will stop motors
const int MOTOR_WEST = 255;     // Amplitude = PWM, sign = direction
const int MOTOR_EAST = -255;      // Amplitude = PWM, sign = direction
const int MOTOR_INTERVAL = 500; // update motor controls every x milliseconds
const int PRINT_INTERVAL = 1000;  // print status to serial interface every x milliseconds

// GLOBALS
unsigned long timeMotor = 0;
unsigned long timeLDR = 0;
unsigned long timePrint = 0;
int ldrEast = 0;
int ldrWest = 0;
int motor_direction = 0;

// light dependant resistor measurememt function
// expects a byte containing the pin to read
// read the analog value of the pin several times, filtering the value to be more repeatable
// protection: check the value of the pin is not full scale or zero, indicating a sensor error
//    if protection invoked, return an error message
// otherwise return the result
int read_ldr(byte pin)
{
  int ldr_average = analogRead(pin);
  for (byte i = 0; i < 5; i++)
  {
    int value = analogRead(pin);
    //    if (value == 0 || value == 1023)
    //      return -1;
    ldr_average += value;  // ldr_average = ldr_average + value;
    ldr_average /= 2;      // ldr_average = ldr_average / 2;
  }
  return ldr_average;
}

// motor control function
// expects a signed integer to define direction and duty to drive the motor
// if the parameter is out of range or zero then we stop motors
// if the parameter is greater than zero we drive to the west
// if the parameter is less than zer we drive to the east
// protection: we don't drive in the direction of a limit switch, when it is active.
// returns what happened
int motor_control(int dir)
{
  // stop condition
  digitalWrite(PIN_LED, LOW);
  if (dir == 0 || dir > 255 || dir < -255)
  {
    // stop motors
    digitalWrite(PIN_MOTOR_ENABLE, LOW);
    digitalWrite(PIN_MOTOR_DRIVE_EAST, LOW);
    digitalWrite(PIN_MOTOR_DRIVE_WEST, LOW);
    motor_direction = MOTOR_STOP;
    digitalWrite(PIN_LED, HIGH);
    return 0;
  }

  // greater than zero -> drive West
  if (dir > 0)
  {
    // if on West limit or driving East (<0), stop
    if (digitalRead(PIN_LIMIT_WEST) || motor_direction < 0)
    {
      motor_control(MOTOR_STOP);
      return 2;
    }
    // drive west
    analogWrite(PIN_MOTOR_ENABLE, MOTOR_WEST);
    digitalWrite(PIN_MOTOR_DRIVE_EAST, HIGH);
    digitalWrite(PIN_MOTOR_DRIVE_WEST, LOW);
    motor_direction = MOTOR_WEST;
    return 1;
  }

  // less than zero -> drive East
  if (dir < 0)
  {
    // if East limit or currently driving west (>0), stop
    if (digitalRead(PIN_LIMIT_EAST) || motor_direction > 0)
    {
      motor_control(MOTOR_STOP);
      return -2;
    }
    // drive east
    analogWrite(PIN_MOTOR_ENABLE, abs(MOTOR_EAST));
    digitalWrite(PIN_MOTOR_DRIVE_EAST, LOW);
    digitalWrite(PIN_MOTOR_DRIVE_WEST, HIGH);
    motor_direction = MOTOR_EAST;
    return -1;
  }
}

// the setup routine runs once when you press reset:
void setup()
{
  // digital outputs
  pinMode(PIN_LED, OUTPUT);

  // digital pwm outputs
  pinMode(PIN_MOTOR_DRIVE_EAST, OUTPUT);
  pinMode(PIN_MOTOR_DRIVE_WEST, OUTPUT);
  pinMode(PIN_MOTOR_ENABLE, OUTPUT);

  // analog inputs
  pinMode(PIN_LDR_EAST, INPUT);
  pinMode(PIN_LDR_WEST, INPUT);

  // digital inputs
  pinMode(PIN_LIMIT_EAST, INPUT);
  pinMode(PIN_LIMIT_WEST, INPUT);
  digitalWrite(PIN_LIMIT_EAST, HIGH);   // enable internal pull-up resistor
  digitalWrite(PIN_LIMIT_WEST, HIGH);   // enable internal pull-up resistor

  Serial.begin(9600);
}

// the loop routine runs over and over again forever:
void loop()
{
  unsigned long time = millis();
  // check LDRs
  if (timeLDR <= time)
  {
    timeLDR = time + LDR_INTERVAL;
    ldrEast = read_ldr(PIN_LDR_EAST);
    ldrWest = read_ldr(PIN_LDR_WEST);
  }

  // do motor control
  if (timeMotor <= time)
  {
    timeMotor = time + MOTOR_INTERVAL;
    if (ldrEast < 0 || ldrWest < 0)
    {
      // stop everything
      // wait 1 minute
      // continue
      motor_control(MOTOR_STOP);
      Serial.print("Motor stop");
      timeMotor = time + 6000;
      motor_control(MOTOR_EAST);
      Serial.print("Pöörame itta");
      delay(5000);
    }
    else
    {
      // main operation
      if (abs(ldrEast - ldrWest) < LDR_DEAD_BAND && ldrEast > LDR_NIGHT && ldrWest > LDR_NIGHT)
      //if (abs(ldrEast - ldrWest) < LDR_DEAD_BAND)
        motor_control(MOTOR_STOP);
      else if (ldrEast < ldrWest) // when west is more illuminated than east
        motor_control(MOTOR_WEST);
      else if (ldrWest < ldrEast)
        motor_control(MOTOR_EAST);
      else if (ldrEast < LDR_NIGHT && ldrWest < LDR_NIGHT)
        motor_control(MOTOR_EAST);

    }
  }

  // print to serial interface
  if (timePrint <= time)
  {
    timePrint = time + PRINT_INTERVAL;
    byte limitEast = digitalRead(PIN_LIMIT_EAST);
    byte limitWest = digitalRead(PIN_LIMIT_WEST);
    Serial.print("East LDR, ");
    Serial.print(ldrEast, DEC);
    Serial.print(",West LDR, ");
    Serial.print(ldrWest, DEC);
    Serial.print(",Lõpus E-W, ");
    Serial.print(limitEast);
    Serial.print(",");
    Serial.print(limitWest);
    Serial.print(",Drive, ");
    Serial.print(motor_direction, DEC);
    // Serial.print(",Panel,");
    Serial.println();

  }
}