Auto5_002

#include <IRremote.h>
#include <EEPROM.h>

#define irPin 11
#define go_time 1000
#define turn_time 200
#define motor_left_A 2
#define motor_left_B 3
#define motor_right_A 4
#define motor_right_B 5
#define reset_button 6
#define pin_for_sign_post_left 7
#define pin_for_sign_post_right 8

IRrecv irrecv(irPin);
decode_results results;
unsigned long current_ms;

unsigned long sign_post_left_star_ms;
unsigned long sign_post_right_star_ms;
const unsigned long sign_post_period = 666;
byte sign_post_left_enable = 0;
byte sign_post_right_enable = 0;

unsigned long go_start_ms;
const unsigned long go_period = 500;
byte go_enable = 0;

unsigned long back_start_ms;
const unsigned long back_period = 500;
byte back_enable = 0;

unsigned long left_start_ms;
const unsigned long left_period = 270;
byte left_enable = 0;

unsigned long right_start_ms;
const unsigned long right_period = 270;
byte right_enable = 0;

const uint32_t code_go = 50190375;
const uint32_t code_back = 50198535;
const uint32_t code_left = 50165895;
const uint32_t code_right = 50157735;
const uint32_t code_sign_left = 50167935;
const uint32_t code_sign_right = 50184255;
const uint32_t code_sign_both = 50151615;



//Funcje

void EEPROMWritelong(int address, long value)
      {
      //Decomposition from a long to 4 bytes by using bitshift.
      //One = Most significant -> Four = Least significant byte
      byte four = (value & 0xFF);
      byte three = ((value >> 8) & 0xFF);
      byte two = ((value >> 16) & 0xFF);
      byte one = ((value >> 24) & 0xFF);

      //Write the 4 bytes into the eeprom memory.
      EEPROM.write(address, four);
      EEPROM.write(address + 1, three);
      EEPROM.write(address + 2, two);
      EEPROM.write(address + 3, one);
      }

long EEPROMReadlong(long address)
      {
      //Read the 4 bytes from the eeprom memory.
      long four = EEPROM.read(address);
      long three = EEPROM.read(address + 1);
      long two = EEPROM.read(address + 2);
      long one = EEPROM.read(address + 3);

      //Return the recomposed long by using bitshift.
      return ((four << 0) & 0xFF) + ((three << 8) & 0xFFFF) + ((two << 16) & 0xFFFFFF) + ((one << 24) & 0xFFFFFFFF);
      }



void setup() {
  Serial.begin(9600);
  irrecv.enableIRIn();
  pinMode(motor_left_A, OUTPUT);
  pinMode(motor_left_B, OUTPUT);
  pinMode(motor_right_A, OUTPUT);
  pinMode(motor_right_B, OUTPUT);
  pinMode(reset_button, INPUT);
  pinMode(pin_for_sign_post_left, OUTPUT);
  pinMode(pin_for_sign_post_right, OUTPUT);
  if (digitalRead(reset_button) == LOW) {
    reset();
  }
}

void go() {
  digitalWrite(motor_left_A, LOW);
  digitalWrite(motor_left_B, HIGH);
  digitalWrite(motor_right_A, LOW);
  digitalWrite(motor_right_B, HIGH);
}

void back() {
  digitalWrite(motor_left_A, HIGH);
  digitalWrite(motor_left_B, LOW);
  digitalWrite(motor_right_A, HIGH);
  digitalWrite(motor_right_B, LOW);
}

void left() {
  digitalWrite(motor_left_A, LOW);
  digitalWrite(motor_left_B, HIGH);
  digitalWrite(motor_right_A, HIGH);
  digitalWrite(motor_right_B, LOW);
}

void right() {
  digitalWrite(motor_left_A, HIGH);
  digitalWrite(motor_left_B, LOW);
  digitalWrite(motor_right_A, LOW);
  digitalWrite(motor_right_B, HIGH);
}

void stop() {
  digitalWrite(motor_left_A, LOW);
  digitalWrite(motor_left_B, LOW);
  digitalWrite(motor_right_A, LOW);
  digitalWrite(motor_right_B, LOW);
}

void sign_post_left() {
  if (sign_post_left_enable == 0x1 && sign_post_right_enable == 0x1 ) {
    sign_post_left_enable = 0x0;
    sign_post_right_enable = 0x0;
    digitalWrite(pin_for_sign_post_left, LOW);
    digitalWrite(pin_for_sign_post_right, LOW);
  }
  switch (sign_post_left_enable) {
      case 0x0 :
      sign_post_left_enable = 1;
      sign_post_right_enable = 0;
      break;
      case 0x1 :
      sign_post_left_enable = 0;
      sign_post_right_enable = 0;
      digitalWrite(pin_for_sign_post_left, LOW);
      digitalWrite(pin_for_sign_post_right, LOW);
      break;
  }
}

void sign_post_right() {
  if (sign_post_left_enable == 0x1 && sign_post_right_enable == 0x1 ) {
    sign_post_left_enable = 0x0;
    sign_post_right_enable = 0x0;
    digitalWrite(pin_for_sign_post_left, LOW);
    digitalWrite(pin_for_sign_post_right, LOW);
  }
  switch (sign_post_right_enable) {
      case 0x0 :
      sign_post_right_enable = 1;
      sign_post_left_enable = 0;
      break;
      case 0x1 :
      sign_post_right_enable = 0;
      sign_post_left_enable = 0;
      digitalWrite(pin_for_sign_post_right, LOW);
      digitalWrite(pin_for_sign_post_left, LOW);
      break;
  }
}

void sign_post_both() {
//  if (sign_post_left_enable == 0x1 || sign_post_right_enable == 0x1 ) {
//    sign_post_left_enable = 0x0;
//    sign_post_right_enable = 0x0;
//    digitalWrite(pin_for_sign_post_left, LOW);
//    digitalWrite(pin_for_sign_post_right, LOW);
//  }
  switch (sign_post_right_enable) {
      case 0x0 :
      sign_post_right_enable = 1;
      sign_post_left_enable = 1;
      break;
      case 0x1 :
      sign_post_right_enable = 0;
      sign_post_left_enable = 0;
      digitalWrite(pin_for_sign_post_right, LOW);
      digitalWrite(pin_for_sign_post_left, LOW);
      break;
  }
}

void reset() {

  long address=0;

  Serial.println("Programowanie pilota");

  byte zaprogramowano = 0;

  Serial.println("Programowanie: przod");
  zaprogramowano = 0;
  go();
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(0, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      zaprogramowano = 1;
    }
  }

  Serial.println("Programowanie: tyl");
  zaprogramowano = 0;
  back();
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(4, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      zaprogramowano = 1;
    }
  }

  Serial.println("Programowanie: prawo");
  zaprogramowano = 0;
  right();
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(12, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      zaprogramowano = 1;
    }
  }

  Serial.println("Programowanie: lewo");
  zaprogramowano = 0;
  left();
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(8, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      zaprogramowano = 1;
    }
  }

  Serial.println("Programowanie: kierunkowskaz lewy");
  zaprogramowano = 0;
  digitalWrite(pin_for_sign_post_left, 1);
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(16, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      digitalWrite(pin_for_sign_post_left, 0);
      zaprogramowano = 1;
    }
  }

  Serial.println("Programowanie: kierunkowskaz prawy");
  zaprogramowano = 0;
  digitalWrite(pin_for_sign_post_right, 1);
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(20, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      digitalWrite(pin_for_sign_post_right, 0);
      zaprogramowano = 1;
    }
  }

  Serial.println("Programowanie: awaryjne");
  zaprogramowano = 0;
  digitalWrite(pin_for_sign_post_left, 1);
  digitalWrite(pin_for_sign_post_right, 1);
  while (zaprogramowano == 0) {
    if (irrecv.decode(&results)) {
      EEPROMWritelong(24, results.value);
      stop();
      delay(1000);
      irrecv.resume();
      Serial.println("Zaprogramowano");
      digitalWrite(pin_for_sign_post_left, 0);
      digitalWrite(pin_for_sign_post_right, 0);
      zaprogramowano = 1;
    }
  }

stop();


  Serial.println("Odczytuje z EEPROM:");
  address=0;
  Serial.println(EEPROMReadlong(address));
  address+=4;
  Serial.println(EEPROMReadlong(address));
  address+=4;
  Serial.println(EEPROMReadlong(address));
  address+=4;
  Serial.println(EEPROMReadlong(address));
  address+=4;
  Serial.println(EEPROMReadlong(address));
  address+=4;
  Serial.println(EEPROMReadlong(address));
  address+=4;
  Serial.println(EEPROMReadlong(address));
}



void loop() {

  if (irrecv.decode(&results)) {

  //Serial.print("0x");
  //Serial.println(results.value);
  //delay(250);


  // sprawdzanie kodow RC5

  if (results.value == EEPROMReadlong(0)) {
      Serial.println("go");
      go_enable = 1;
      go_start_ms = millis();
      go();
  }

  if (results.value == EEPROMReadlong(4)) {
      Serial.println("back");
      back_enable = 1;
      back_start_ms = millis();
      back();
  }

  if (results.value == EEPROMReadlong(8)) {
      Serial.println("left");
      left_enable = 1;
      left_start_ms = millis();
      left();
  }

  if (results.value == EEPROMReadlong(12)) {
      Serial.println("right");
      right_enable = 1;
      right_start_ms = millis();
      right();
  }

  if (results.value == EEPROMReadlong(16)) {
      Serial.println("sign post left");
      sign_post_left();
  }

  if (results.value == EEPROMReadlong(20)) {
      Serial.println("sign post right");
      sign_post_right();
  }

  if (results.value == EEPROMReadlong(24)) {
      Serial.println("sign post both");
      sign_post_both();
  }

    irrecv.resume();
  }

// obsluga kierunkowskazu
  if (sign_post_left_enable == 0x1 || sign_post_right_enable == 0x1 ) {
    current_ms = millis();  //pobierz i zapisz do zmiennej liczbę milisekund jaka upłynęła od czasu włączenia Arduino
    if (current_ms - sign_post_left_star_ms >= sign_post_period)  //jeśli aktualna liczba milisekund po odjęciu startowych milisekund przekracza liczbę zadeklarowaną w zmiennej sign_post_period milisekund
    {
      if (sign_post_left_enable == 0x1) { digitalWrite(pin_for_sign_post_left, !digitalRead(pin_for_sign_post_left));  //przełącz stan kierunkowskazu
      }
      if (sign_post_right_enable == 0x1) { digitalWrite(pin_for_sign_post_right, !digitalRead(pin_for_sign_post_right));  //przełącz stan kierunkowskazu
      }
      sign_post_left_star_ms = current_ms;  //ustaw liczbę startowych milisekund na bieżącą wartość milisekund (od startu Arduino)
    }
  }

// obsluga jazdy do przodu
  if (go_enable == 0x1) {
    current_ms = millis();  //pobierz i zapisz do zmiennej liczbę milisekund jaka upłynęła od czasu włączenia Arduino
    if (current_ms - go_start_ms >= go_period)  //jeśli aktualna liczba milisekund po odjęciu startowych milisekund przekracza liczbę zadeklarowaną w zmiennej period milisekund
    {
      stop();
      go_enable = 0;
    }
  }


// obsluga jazdy do tyłu
  if (back_enable == 0x1) {
    current_ms = millis();  //pobierz i zapisz do zmiennej liczbę milisekund jaka upłynęła od czasu włączenia Arduino
    if (current_ms - back_start_ms >= go_period)  //jeśli aktualna liczba milisekund po odjęciu startowych milisekund przekracza liczbę zadeklarowaną w zmiennej period milisekund
    {
      stop();
      back_enable = 0;
    }
  }


// obsluga jazdy w prawo
  if (right_enable == 0x1) {
    current_ms = millis();  //pobierz i zapisz do zmiennej liczbę milisekund jaka upłynęła od czasu włączenia Arduino
    if (current_ms - right_start_ms >= right_period)  //jeśli aktualna liczba milisekund po odjęciu startowych milisekund przekracza liczbę zadeklarowaną w zmiennej period milisekund
    {
      stop();
      right_enable = 0;
    }
  }

// obsluga jazdy w lewo
  if (left_enable == 0x1) {
    current_ms = millis();  //pobierz i zapisz do zmiennej liczbę milisekund jaka upłynęła od czasu włączenia Arduino
    if (current_ms - left_start_ms >= left_period)  //jeśli aktualna liczba milisekund po odjęciu startowych milisekund przekracza liczbę zadeklarowaną w zmiennej period milisekund
    {
      stop();
      left_enable = 0;
    }
  }

}