#include <Arduino.

h>
#include <BluetoothSerial.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)

#error Bluetooth is not enabled! Please run make menuconfig to and enable it
#endif
BluetoothSerial SerialBT;
#include <QTRSensors.h> //Pololu QTR Sensor Library. First you must download and
install QTRSensors library

bool trangthai;
int blue;
int d,b,c;
int mode;

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////* Khai báo động cơ bánh xe
*///////////////////////////////////////////////////////
#define PWM_FREQ 1000
#define PWM_RES 8

#define Drv_In1_pin 23
#define Drv_In2_pin 16// PWM
#define Drv_In2_chn 2

#define Drv_In3_pin 17
#define Drv_In4_pin 15 // PWM
#define Drv_In4_chn 3

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////* Khai báo động cơ rulo
*///////////////////////////////////////////////////////
#define Drv_rulo_pin 18
#define Drv_rulo1_pin 0// PWM
#define Drv_rulo_chn 4
int motor_speed = 255; // tốc độ tốc độ motor
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////* Khai báo servo tay gắp
*//////////////////////////////////////////////////////////
#define servoFreq 50
#define servoResolution 11
#define servoGap_channel 6
#define servoGap 12 // UP/down
#define servoNang_channel 7
#define servoBong_channel 10
#define servoNang 13
#define servoXuc_channel 8
#define servoXuc 5
#define servoBong 14
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////*khai báo cảm biến dò line
*//////////////////////////////////////////////////////////
/*Line ss*/
//Khai báo dò line
#define Kp 0.65 //0.015 //0.015 //0.017 //0.04 // experiment to determine
this, start by something small that just makes your bot follow the line at a slow
speed
#define Kd 0.35 //0.34 //0.36 //0.35 //0.2 // experiment to determine this,
slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
#define MaxSpeed 255 // max speed of the robot
#define BaseSpeed 90
#define DEBUG 0
#define On_calb_led_pin 19
#define Line_ss_enable_pin 21
#define SensorCount 5
#define SensorInputPin {32,33,25,26,27}

uint16_t sensorValues[SensorCount];
QTRSensors qtr;

bool line; // giá trị trả về của cảm biến

#define spd 125 // chỉnh tốc

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////* các hàm con điều khiển
*/////////////////////////////////////////////////////////

void tien() {
digitalWrite(Drv_In1_pin, 0);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In2_chn, motor_speed); // PWM_1_pin
ledcWrite(Drv_In4_chn, motor_speed); // PWM_2_pin
}

void xuc() {
// ledcWrite(servoBong_channel, 50);
for (int i = 50; i <= 155; i++) {
ledcWrite(servoXuc_channel, i);
delay(8); // Adjust delay as needed
}
}
void nang() {
ledcWrite(servoNang_channel, 50);
}
void ha() {
ledcWrite(servoNang_channel, 170);
}
void Bong() {
ledcWrite(servoBong_channel, 50); // servo ngan bong
delay(120);
ledcWrite(servoBong_channel, 65);
}
void xucha() {
// ledcWrite(servoG_channel, 155);
for (int i = 155; i >= 50; i--) {
ledcWrite(servoXuc_channel, i); // servo ha xuc bong
delay(10);
}
}
void gap() {
ledcWrite(servoGap_channel, 50); // gap
}
void nha() {
 ledcWrite(servoGap_channel, 102); // nha
}
void lui() {
digitalWrite(Drv_In1_pin, 1);
// print the calibration maximum values measured when emitters were on
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.maximum[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();

// #endif
delay(1000);
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////* dò line
*//////////////////////////////////////////////////////////
int lastError = 0;
unsigned int sensors[5];
int position = 2000; //qtrrc.readLine(sensors);

void doline()
{

int32_t LinePosition = qtr.readLineBlack(sensorValues);

int error = LinePosition - 2000;
if ((error < 800) && (error > -800))
tien_1();
else if ((error > 300) && (error < 900))
rephai_1();
else if ((error < -300) && (error > -900))
retrai_1();
else if ((error > 900) && (error < 1600))
rephai1();
else if ((error < -900) && (error > -1600))
retrai1();
else if (error > 1600)
rephai2();
else if (error < -1600)
retrai2();
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
///////////////////////////////* điều khiển (tự set lại các case)
*///////////////////////////////////////////////
void dieukhien() {

//Serial.println(motor_speed);
//Serial.println(blue);
switch (blue) {
case 0:
dung();
break;
case 1:
 tien();
break;
case 2:
rephai();
break;
case 3:
lui();
break;
case 4:
retrai();
break;
case 9:
ban();
break;
case 10:
ngung();
break;
case 11:
xuc();
break;
case 12:
xucha();
break;
case 13:
nang();
break;
case 14:
ha();
break;
case 15:
bong();
break;
case 16:
bong();
break;
case 21:
mode = false;
break;
case 20:
mode = true;
break;
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////
void setup() {
Serial.begin(9600);
SerialBT.begin("IEC-Elitebots"); // Bluetooth device name

// dong co
ledcSetup(servoGap_channel, servoFreq, 11);
ledcSetup(servoNang_channel, servoFreq, servoResolution);
ledcSetup(servoday_channel, servoFreq, servoResolution);
ledcAttachPin(servoNang , servoNang_channel);
ledcAttachPin(servoGap, servoGap_channel);
ledcAttachPin(servoday , servoday_channel);
LineSensorInit();
 //motor1
pinMode(Drv_In1_pin,OUTPUT);
ledcSetup(Drv_In2_chn, PWM_FREQ, PWM_RES);
ledcAttachPin(Drv_In2_pin, Drv_In2_chn);
//motor2
pinMode(Drv_In3_pin,OUTPUT);
ledcSetup(Drv_In4_chn, PWM_FREQ, PWM_RES);
ledcAttachPin(Drv_In4_pin, Drv_In4_chn);
//rulo
pinMode(Drv_rulo_pin,OUTPUT);
ledcSetup(Drv_rulo_chn, PWM_FREQ, PWM_RES);
ledcAttachPin(Drv_rulo1_pin, Drv_rulo_chn);
}

void loop() {
ledcWrite(servoGap_channel, d);
ledcWrite(servoNang_channel, b);
ledcWrite(servoday_channel, c);
if (SerialBT.available()) {
blue = SerialBT.read();
//Serial.println(blue);
if(blue >= 60 && blue <= 111) {
motor_speed = (blue - 60) * 5;
}
if (blue==20)
mode=1;
else if (blue==21)
mode=0;

}
if (mode==0) {
doline();
} else {
dieukhien();
}
}
digitalWrite(Drv_In3_pin, 1);
ledcWrite(Drv_In2_chn, motor_speed); // PWM_1_pin
ledcWrite(Drv_In4_chn, motor_speed); // PWM_2_pin
}
void retrai() {
digitalWrite(Drv_In1_pin, HIGH);
digitalWrite(Drv_In3_pin, LOW);
ledcWrite(Drv_In2_chn, motor_speed); // PWM_1_pin
ledcWrite(Drv_In4_chn, motor_speed); // PWM_2_pin
}
void rephai() {
digitalWrite(Drv_In1_pin, LOW);
digitalWrite(Drv_In3_pin, HIGH);
ledcWrite(Drv_In2_chn, motor_speed); // PWM_1_pin
ledcWrite(Drv_In4_chn, motor_speed); // PWM_2_pin
}
void dung() {
ledcWrite(Drv_In2_chn, 0); // PWM_1_pin
ledcWrite(Drv_In4_chn, 0); // PWM_2_pin
}

void ban() {
 digitalWrite(Drv_rulo_pin, 0);
ledcWrite(Drv_rulo_chn, 150);
}
void ngung() {
digitalWrite(Drv_rulo_pin, 0);
ledcWrite(Drv_rulo_chn, 0);
}

void tien_1() {
digitalWrite(Drv_In1_pin, 0);
ledcWrite(Drv_In2_chn, spd-50);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In4_chn, spd-50);
}

void rephai_1() {
digitalWrite(Drv_In1_pin, 0);
ledcWrite(Drv_In2_chn, spd);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In4_chn, spd-30);
}

void retrai_1() {
digitalWrite(Drv_In1_pin, 0);
ledcWrite(Drv_In2_chn, spd-30);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In4_chn, spd);
}

void rephai1() {
digitalWrite(Drv_In1_pin, 0);
ledcWrite(Drv_In2_chn, spd);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In4_chn, spd-50);
}

void retrai1() {
digitalWrite(Drv_In1_pin, 0);
ledcWrite(Drv_In2_chn, spd-50);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In4_chn, spd);
}

void rephai2() {
digitalWrite(Drv_In1_pin, 0);
ledcWrite(Drv_In2_chn, spd);
digitalWrite(Drv_In3_pin, 1);
ledcWrite(Drv_In4_chn, spd);
}

void retrai2() {
digitalWrite(Drv_In1_pin, 1);
ledcWrite(Drv_In2_chn, spd);
digitalWrite(Drv_In3_pin, 0);
ledcWrite(Drv_In4_chn, spd);
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
////////////////////////////////* hàm quét các cảm biến dò line
*/////////////////////////////////////////////////
void LineSensorInit() {
qtr.setTypeRC();
qtr.setSensorPins((const uint8_t[])SensorInputPin, SensorCount);
qtr.setEmitterPin(Line_ss_enable_pin);
delay(500);
pinMode(On_calb_led_pin, OUTPUT);
digitalWrite(On_calb_led_pin, HIGH); // turn on Arduino's LED to indicate we are
in calibration mode
// 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
// = ~25 ms per calibrate() call.
// Call calibrate() 400 times to make calibration take about 10 seconds.
for (uint16_t i = 0; i < 400; i++)
{
qtr.calibrate();
}
digitalWrite(On_calb_led_pin, LOW); // turn off Arduino's LED to indicate we are
through with calibration
// #if DEBUG
// print the calibration minimum values measured when emitters were on
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.minimum[i]);
Serial.print(' ');
}
Serial.println();