// Author : Mason, masonchen1003@gmail.com

// FB : https://www.facebook.com/mason.chen.1420

// Using ESP32CAM + Gyro shield + L9110 to control two N20 motors

// https://sites.google.com/view/esp-gyro/

// Android App: 尤老師提供的 Arduino Bluetooth Controller

#include "soc/soc.h" //disable brownout problems

#include "soc/rtc_cntl_reg.h" //disable brownout problems

#include "BluetoothSerial.h"

BluetoothSerial SerialBT;

// Gyro 擴充板左下方插上 L9110 ，並定義使用的腳位如下

int motor_A1 = 12;

int motor_B1 = 13;

int motor_A2 = 14;

int motor_B2 = 15;

void backward() {

digitalWrite(motor_A1, HIGH);

digitalWrite(motor_B1, LOW);

digitalWrite(motor_A2, HIGH);

digitalWrite(motor_B2, LOW);

}

void forward() {

digitalWrite(motor_A1, LOW);

digitalWrite(motor_B1, HIGH);

digitalWrite(motor_A2, LOW);

digitalWrite(motor_B2, HIGH);

}

void turn_right() {

digitalWrite(motor_A1, HIGH);

digitalWrite(motor_B1, LOW);

digitalWrite(motor_A2, LOW);

digitalWrite(motor_B2, HIGH);

}

void turn_left() {

digitalWrite(motor_A1, LOW);

digitalWrite(motor_B1, HIGH);

digitalWrite(motor_A2, HIGH);

digitalWrite(motor_B2, LOW);

}

void stop_all() {

digitalWrite(motor_A1, LOW);

digitalWrite(motor_B1, LOW);

digitalWrite(motor_A2, LOW);

digitalWrite(motor_B2, LOW);

}

String cmdaaa ="";

void setup() {

WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector

pinMode(4, OUTPUT); // ESP32CAM 閃光燈

pinMode(12, OUTPUT); // motor-A1

pinMode(13, OUTPUT); // motor-B1

pinMode(14, OUTPUT); // motor-A2

pinMode(15, OUTPUT); // motor-B2

Serial.begin(115200);

SerialBT.begin("Gyro_robot"); //Bluetooth device name

}

void loop() {

while(SerialBT.available()>0){

cmdaaa +=(char)SerialBT.read();

}

if (cmdaaa == "u") { // 向上

forward();

}

if (cmdaaa == "d") { // 向下

backward();

}

if (cmdaaa == "l") { // 向左

turn_left();

}

if (cmdaaa == "r") { // 向右

turn_right();

}

if (cmdaaa == "s") { // 確認鍵

stop_all();

}

if (cmdaaa == "a") { // A 鍵

digitalWrite(4, HIGH); //Turn on

}

if (cmdaaa == "b") { // B 鍵

digitalWrite(4, LOW); //Turn off

}

cmdaaa="";

}