AGV_Control_Backup

#include <EEPROM.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Keypad_I2C.h>

#define interrupt_pin1  2
#define interrupt_pin2  3

#define left_pin_signal  5
#define middle_pin_signal  6
#define right_pin_signal  7

#define direct_left 9
#define direct_right 10
#define pulse_left 11
#define pulse_right 12

#define left_current_signal  A0

#define middle_current_signal  A1
#define right_current_signal  A2

#define numRows 4
#define numCols 4

volatile boolean exit_loop = false;

boolean stat = false;

char keymap[numRows][numCols] =
{
 {'1', '2', '3', 'A'},
 {'4', '5', '6', 'B'},
 {'7', '8', '9', 'C'},
 {'*', '0', '#', 'D'}
};
byte rowPins[numRows] = {0, 1, 2, 3};
byte colPins[numCols] = {4, 5, 6, 7};

volatile byte agv_direct = 0; //0-forward; 1-left; 2-right

volatile boolean obstacle_detected_flg = false;
boolean motor_reverse[2];

LiquidCrystal_I2C lcd (0x27, 20, 4);

Keypad_I2C myKeypad = Keypad_I2C(makeKeymap(keymap), rowPins, colPins,
numRows, numCols, 0x20);
//Keypad myKeypad = Keypad(makeKeymap(keymap), rowPins, colPins, numRows,
numCols);

void setup() {
 // put your setup code here, to run once:
 Serial.begin(9600);
 myKeypad.begin( makeKeymap(keymap) );
 lcd.begin();
 lcd.backlight();
 //  attachInterrupt(digitalPinToInterrupt(interrupt_pin1),
instruction_received, RISING);
 //  attachInterrupt(digitalPinToInterrupt(interrupt_pin2),
obstacle_detected, RISING);
 //  attachInterrupt(digitalPinToInterrupt(interrupt_pin2),
obstacle_not_detected, FALLING);

 pinMode(left_pin_signal, INPUT);
 pinMode(middle_pin_signal, INPUT);
 pinMode(right_pin_signal, INPUT);

 pinMode(left_current_signal, OUTPUT);
 pinMode(middle_current_signal, OUTPUT);
 pinMode(right_current_signal, OUTPUT);

 pinMode(direct_left, OUTPUT);
 pinMode(direct_right, OUTPUT);
 pinMode(pulse_left, OUTPUT);
 pinMode(pulse_right, OUTPUT);

 motor_reverse[0] = 0;
 motor_reverse[1] = 0;

 agv_direct = 0;

 backToHome();
}

void loop() {
 // put your main code here, to run repeatedly:
 char keypressed = myKeypad.getKey();
 if (keypressed != NO_KEY) {
   if (keypressed == '1') {
     run_options();
     backToHome();
   }
   else if (keypressed == '2') {
     settings_options();
     backToHome();
   }
 }
}

void display_run_options() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print("      RUN AGV      ");
 lcd.setCursor(0, 1);
 lcd.print("1.Stop at End");
 lcd.setCursor(0, 2);
 lcd.print("2.Continuous");
}

void run_options() {
 display_run_options();
 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == 'C') {
       exit_loop = true;
     }
     else if (keypressed == '1') {
       //        run_stop_at_end();
     }
     else if (keypressed == '2') {
       //        run_continuous();
     }
   }
 }
 exit_loop = false;
}

void display_settings_options() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("      SETTINGS      "));
 lcd.setCursor(0, 1);
 lcd.print(F("1.Set Speed"));
 lcd.setCursor(0, 2);
 lcd.print(F("2.Invert Wheel"));
 lcd.setCursor(0, 3);
 lcd.print(F("3.Calibrate"));
}

void settings_options() {
 display_settings_options();
 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == 'C') {
       exit_loop = true;
     }
     else if (keypressed == '1') {
       //        set_speed_options();
     }
     else if (keypressed == '2') {
       //        invert_wheel_options();
     }
     else if (keypressed == '3') {
       //        calibrate_options();
     }
   }
 }
 exit_loop = false;
}

void run_continuous() {
 if (!obstacle_detected_flg) {
   if (agv_direct == 0) run_motor_forward();
   else if (agv_direct == 1) run_motor_left();
   else if (agv_direct == 2) run_motor_right();
 }
}
void run_motor_forward() {
 while (agv_direct == 0) {
   digitalWrite(pulse_left, HIGH);
   digitalWrite(pulse_right, HIGH);
   delayMicroseconds(1000);
   digitalWrite(pulse_left, LOW);
   digitalWrite(pulse_right, LOW);
   delayMicroseconds(1000);
 }
}

void run_motor_left() {
 while (agv_direct == 1) {
   digitalWrite(pulse_right, HIGH);
   delayMicroseconds(1000);
   digitalWrite(pulse_right, LOW);
   delayMicroseconds(1000);
 }
}

void run_motor_right() {
 while (agv_direct == 2) {
   digitalWrite(pulse_left, HIGH);
   delayMicroseconds(1000);
   digitalWrite(pulse_left, LOW);
   delayMicroseconds(1000);
 }
}

void instruction_received() {
 if (stat) {
   Serial.println("void instruction_received");
   if (digitalRead(middle_pin_signal)) {
     agv_direct = 0;
     digitalWrite(left_current_signal, LOW);
     digitalWrite(right_current_signal, LOW);
     digitalWrite(middle_current_signal, HIGH);
   }
   else if (digitalRead(left_pin_signal)) {
     agv_direct = 1;
     digitalWrite(right_current_signal, LOW);
     digitalWrite(middle_current_signal, LOW);
     digitalWrite(left_current_signal, HIGH);
   }
   else if (digitalRead(right_pin_signal)) {
     agv_direct = 2;
     digitalWrite(left_current_signal, LOW);
     digitalWrite(middle_current_signal, LOW);
     digitalWrite(right_current_signal, HIGH);
   }
 }
}

void obstacle_detected() {
 if (stat) {
   Serial.println("void obstacle_detected");
   obstacle_detected_flg = 1;
 }
}

void obstacle_not_detected() {
 if (stat) {
   Serial.println("void obstacle_not_detected");
   obstacle_detected_flg = 0;
 }
}

void backToHome() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("    AGV OPTIONS    "));
 lcd.setCursor(0, 1);
 lcd.print(F("1.Run"));
 lcd.setCursor(0, 2);
 lcd.print(F("2.Settings"));
}

//  machine_running = true;
//
//  boolean stat_z = true;
//  boolean stat_r = true;
//  unsigned long lcount_z = 0;
//  unsigned long lcount_r = 0;
//  unsigned long start_z;
//  unsigned long start_r;
//  unsigned long duration_z = 0;
//  unsigned long duration_r = 0;
//
//  start_z = millis();
//  start_r = start_z;
//  while (1) {
//    duration_z = micros() - start_z;
//    if (duration_z >= z_width_tap && lcount_z < count[2]) {
//      stat_z = !stat_z;
//      start_z = micros();
//      digitalWrite(pulse_pin_z_r, stat_z);
//      if (!stat_z) lcount_z ++;
//    }
//    duration_r = micros() - start_r;
//    if (duration_r >= tap_width && lcount_r < count[3]) {
//      stat_r = !stat_r;
//      start_r = micros();
//      digitalWrite(pulse_pin_r_f, stat_r);
//      if (!stat_r) lcount_r ++;
//    }
//    if (lcount_z == count[2] && lcount_r == count[3]) break;
//  }
//  machine_running = false;
AGV_Motor_Control

#include <EEPROM.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Keypad_I2C.h>

#define direct_left 9
#define direct_right 10
#define pulse_left 11
#define pulse_right 12

#define numRows 4
#define numCols 4

#define interrupt_pin 2

volatile boolean exit_loop = false;

volatile boolean agv_running = false;

boolean invert_left;
byte invert_left_loc = 0;

boolean invert_right;
byte invert_right_loc = 1;

long pulse_width;
byte pulse_width_loc = 100;

char keymap[numRows][numCols] =
{
 {'1', '2', '3', 'A'},
 {'4', '5', '6', 'B'},
 {'7', '8', '9', 'C'},
 {'*', '0', '#', 'D'}
};
byte rowPins[numRows] = {0, 1, 2, 3};
byte colPins[numCols] = {4, 5, 6, 7};

LiquidCrystal_I2C lcd (0x27, 20, 4);

Keypad_I2C myKeypad = Keypad_I2C(makeKeymap(keymap), rowPins, colPins,
numRows, numCols, 0x20);

void setup() {
 // put your setup code here, to run once:
 Serial.begin(9600);
 myKeypad.begin( makeKeymap(keymap) );
 lcd.begin();
 lcd.backlight();
 attachInterrupt(digitalPinToInterrupt(interrupt_pin), stop_agv, RISING);

 pinMode(direct_left, OUTPUT);
 pinMode(direct_right, OUTPUT);
 pinMode(pulse_left, OUTPUT);
 pinMode(pulse_right, OUTPUT);
 invert_left = retrieve_boolean_data(invert_left_loc);
 invert_right = retrieve_boolean_data(invert_right_loc);
 update_invert_motor();

 pulse_width = retrieve_long_data(pulse_width_loc);

 backToHome();
}

void loop() {
 // put your main code here, to run repeatedly:
 char keypressed = myKeypad.getKey();
 if (keypressed != NO_KEY) {
   if (keypressed == '1') {
     run_options();
     backToHome();
   }
   else if (keypressed == '2') {
     settings_options();
     backToHome();
   }
 }
}

void update_invert_motor() {
 if (invert_left) digitalWrite(direct_left, LOW);
 else  digitalWrite(direct_left, HIGH);

 if (invert_right) digitalWrite(direct_right, LOW);

 else  digitalWrite(direct_right, HIGH);
}

void display_run_options() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print("      RUN AGV      ");
 lcd.setCursor(0, 1);
 lcd.print("1.Stop at End");
 lcd.setCursor(0, 2);
 lcd.print("2.Continuous");
}

void run_options() {
 display_run_options();
 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == 'C') {
       exit_loop = true;
     }
     else if (keypressed == '1') {
       //run_stop_at_end();
       display_run_options();
     }
     else if (keypressed == '2') {
       run_continuous();
       display_run_options();
     }
   }
 }
 exit_loop = false;
}

void display_settings_options() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("      SETTINGS      "));
 lcd.setCursor(0, 1);
 lcd.print(F("1.Set Speed"));
 lcd.setCursor(0, 2);
 lcd.print(F("2.Invert Wheel"));
 lcd.setCursor(0, 3);
 lcd.print(F("3.Calibrate"));
}

void settings_options() {
 display_settings_options();
 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == 'C') {
       exit_loop = true;
     }
     else if (keypressed == '1') {
       set_speed_options();
       display_settings_options();
     }
     else if (keypressed == '2') {
       invert_wheel_options();
       display_settings_options();
     }
     else if (keypressed == '3') {
       //calibrate_options();
       display_settings_options();
     }
   }
 }
 exit_loop = false;
}

void invert_wheel_options() {
 byte temp_left = invert_left;
 byte temp_right = invert_right;

 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("Invert Axis?"));

 lcd.setCursor(0, 2);
 lcd.print(F("1.Left: "));
 lcd.print(invert_left);

 lcd.setCursor(0, 3);
 lcd.print(F("2.Right: "));
 lcd.print(invert_right);

 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == 'C') {
       exit_loop = true;
     }
     else if (keypressed == '1') {
       lcd.setCursor(8, 2);
       if (temp_left == 1) temp_left = 0;
       else temp_left = 1;
       lcd.print(temp_left);
       EEPROM.write(invert_left_loc, temp_left);
       invert_left = temp_left;
       update_invert_motor();
     }
     else if (keypressed == '2') {
       lcd.setCursor(9, 3);
       if (temp_right == 1) temp_right = 0;
       else temp_right = 1;
       lcd.print(temp_right);
       EEPROM.write(invert_right_loc, temp_right);
       invert_right = temp_right;
       update_invert_motor();
     }
   }
 }
 exit_loop = false;
}

void display_speed_options() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("   SETTINGS:SPEED   "));
 lcd.setCursor(0, 1);
 lcd.print(F("Current RPM:"));
 lcd.print(pulse_width);
 lcd.setCursor(0, 2);
 lcd.print(F("1.Set RPM"));
}

void set_speed_options() {
 String temp = "";
 long value = 0;
 display_speed_options();
 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == 'C') { //Exit
       exit_loop = true;
     }
     else if (keypressed == '1') {
       temp = enter_value();
       if (temp != "") {
         value = temp.toInt();
         EEPROM_writeAnything(pulse_width_loc, value);
         pulse_width = value;
       }
       display_speed_options();
     }
   }
 }
 exit_loop = false;
}

void run_continuous() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print("Running...");
 Serial.println("Running...");
 agv_running = true;
 while (agv_running) {
   //char key = myKeypad.getKey();
   digitalWrite(pulse_left, HIGH);
   digitalWrite(pulse_right, HIGH);
   delayMicroseconds(pulse_width);
   digitalWrite(pulse_left, LOW);
   digitalWrite(pulse_right, LOW);
   delayMicroseconds(pulse_width);
 }
}

void backToHome() {
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("    AGV OPTIONS    "));
 lcd.setCursor(0, 1);
 lcd.print(F("1.Run"));
 lcd.setCursor(0, 2);
 lcd.print(F("2.Settings"));
}

boolean retrieve_boolean_data(byte data_loc) {

 boolean temp;
 EEPROM_readAnything(data_loc, temp);
 return temp;
}

float retrieve_float_data(byte data_loc) {

 float temp;
 EEPROM_readAnything(data_loc, temp);
 return temp;
}

byte retrieve_byte_data(byte data_loc) {

 byte temp;
 EEPROM_readAnything(data_loc, temp);
 return temp;
}

long retrieve_long_data(byte data_loc) {

 long temp;
 EEPROM_readAnything(data_loc, temp);
 return temp;
}

String enter_value() {
 String temp;
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print(F("Enter new value (us):"));
 lcd.setCursor(0, 2);
 lcd.print(F(">>"));
 lcd.blink();
 while (!exit_loop) {
   char keypressed = myKeypad.getKey();
   if (keypressed != NO_KEY) {
     if (keypressed == '*' || keypressed == '#' || keypressed == 'A' ||
keypressed == 'B') {
       //do nothing
     }
     else if (keypressed == 'C') {
       temp = "";
       exit_loop = true;
     }
     else if (keypressed == 'D') {
       exit_loop = true;
     }
     else {
       temp += keypressed;
       lcd.print(keypressed);
     }
   }
 }
 exit_loop = false;
 lcd.noBlink();
 return temp;
}

template <class T> int EEPROM_writeAnything(int ee, const T & value) {

 const byte* p = (const byte*)(const void*)&value;
 unsigned int i;
 for (i = 0; i < sizeof(value); i++)
   EEPROM.write(ee++, *p++);
 return i;
}

template <class T> int EEPROM_readAnything(int ee, T & value) {

 byte* p = (byte*)(void*)&value;
 unsigned int i;
 for (i = 0; i < sizeof(value); i++)
   *p++ = EEPROM.read(ee++);
 return i;
}

void stop_agv() {
 agv_running = false;
}
AGV_Control_Backup

#define left_pin_signal  5
#define middle_pin_signal  6
#define right_pin_signal  7

#define left_current_signal  A3

#define middle_current_signal  A4
#define right_current_signal  A5

#define instruction_pin  2
#define obstacle_pin  3

void setup() {
 // put your setup code here, to run once:
 pinMode(left_pin_signal, OUTPUT);
 pinMode(middle_pin_signal, OUTPUT);
 pinMode(right_pin_signal, OUTPUT);

 pinMode(left_current_signal, INPUT);
 pinMode(middle_current_signal, INPUT);
 pinMode(right_current_signal, INPUT);

 pinMode(instruction_pin, OUTPUT);
 pinMode(obstacle_pin, OUTPUT);
}

void loop() {
 // put your main code here, to run repeatedly:
 check_obstacle();
 set_direction();
 delay(10);
}

void check_obstacle() {
 boolean distance;

 if (distance < 300) digitalWrite(obstacle_pin, HIGH);

 else digitalWrite(obstacle_pin, LOW);
}

void set_direction() {
 byte current_direct;
 byte new_direct;

 if (digitalRead(middle_current_signal)) current_direct = 0;

 else if (digitalRead(left_current_signal)) current_direct = 1;
 else if (digitalRead(right_current_signal)) current_direct = 2;

 //insert code to check new direct

 if (new_direct != current_direct) {

   if (new_direct == 0) digitalWrite(middle_pin_signal, HIGH);
   else if (new_direct == 1) digitalWrite(left_pin_signal, HIGH);
   else if (new_direct == 2) digitalWrite(right_pin_signal, HIGH);

   digitalWrite(instruction_pin, HIGH);
   delay(1);
   digitalWrite(instruction_pin, LOW);

   digitalWrite(middle_pin_signal, LOW);
   digitalWrite(left_pin_signal, LOW);
   digitalWrite(right_pin_signal, LOW);
 }
}
AGV_Sensor_Control
#define left_motor  3
#define right_motor  4

#define left_sensor  6
#define right_sensor  7

boolean obstacle_detected = false;

void setup() {
 // put your setup code here, to run once:
 pinMode(left_motor, OUTPUT);
 pinMode(right_motor, OUTPUT);

 pinMode(left_sensor, INPUT);
 pinMode(right_sensor, INPUT);
}

void loop() {
 //check_obstacle();
 if (!obstacle_detected) set_direction();
 delay(10);
}

void check_obstacle() {
 boolean distance;

 if (distance < 100) {

   digitalWrite(left_motor, LOW);
   digitalWrite(right_motor, LOW);
   obstacle_detected = true;
 }
 else obstacle_detected = false;
}

void set_direction() {
  if (digitalRead(left_sensor) && digitalRead(right_sensor)) {
   digitalWrite(right_motor, LOW);
   digitalWrite(left_motor, LOW);
 }
 else if (!digitalRead(left_sensor) && !digitalRead(right_sensor)) {
   digitalWrite(right_motor, HIGH);
   digitalWrite(left_motor, HIGH);
 }
 else if (digitalRead(left_sensor)) {
   digitalWrite(right_motor, HIGH);
   digitalWrite(left_motor, LOW);
 }
 else if (digitalRead(right_sensor)) {
   digitalWrite(left_motor, HIGH);
   digitalWrite(right_motor, LOW);
 }
}