#define REMOTEXY_MODE__SOFTSERIAL
#include <SoftwareSerial.h> //джойстик блютуз
#include <RemoteXY.h> //джойстик блютуз
#include <Wire.h>//библиотека работы с цап
#include <SPI.h>

const int ENCODER_PIN_A = 10;    //Пин, к которому подключен энкодер.
const int ENCODER_PIN_B = 11;    //Пин, к которому подключен энкодер.
const int ENCODER_PIN_C = 15;
const int SERVO_DIRECTION_PIN = 20;//Направление серво.

const int PWM_PIN_A = 17;//ПИНЫ управления углами поворота ШИМ
const int PWM_PIN_B = 18;
const int PWM_PIN_C = 19;

const int alpha=86; //максимальный угол поворота серва 30 град
const int beta=71;//макимальный угол 25 град
const int gamma=86;//максимальный угол поворота серва 30 град
//T=20ms
const int PWM_A=50;//10ms
const int PWM_B=40;//8ms работает 12 отдыхет
const int PWM_C=50;
#define PCF8591 0x48//адрес подцепления ЦАП
#define PCF8591_2 0x27//адрес подцепления второго ЦАП
// настройки соединения
#define REMOTEXY_SERIAL_RX 10 //работа с bluetooth
#define REMOTEXY_SERIAL_TX 11 //работа с bluetooth
#define REMOTEXY_SERIAL_SPEED 9600
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
  { 255,3,0,0,0,22,0,8,13,0,
  5,0,10,16,30,30,2,26,31,1,
  0,70,23,12,12,2,31,88,0 };

// структура определяет все переменные вашего интерфейса управления
struct {

    // input variable
  int8_t joystick_1_x; // =-100..100 координата x положения джойстика
  int8_t joystick_1_y; // =-100..100 координата y положения джойстика
  uint8_t button_1; // =1 если кнопка нажата, иначе =0

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0

} RemoteXY;
#pragma pack(pop)

/////////////////////////////////////////////
//           END RemoteXY include          //
/////////////////////////////////////////////


// в сантиметрах (distance threshold) Пороги расстояний до препятствия
// Если ближе, то стоп
const int DST_STOP = 20;
boolean SAFE_DISTANCE = true; // измерение дистанции и безопасное движение. Если впереди близко препятствие, то стоп
int distance = 0;

/* пины для подключения HC-SR04 Ultrasonic Module Distance Measuring
 * 13, 2 цифровые пины
 * 14, 15 аналоговые пины A0 и A1 соответственно
 */
#define SONIC_PIN_TRIG 14 //вывод на парктроник
#define SONIC_PIN_ECHO 15 //вывод на парктроник
#define SONIC_PIN_TRIG_BACK 16 //вывод на парктроник
#define SONIC_PIN_ECHO_BACK 17// вывод на парктроник
#define PIN_BUTTON_STOP 13//вывод кнопки тормоза
// Detection distance: 2cm--450cm
const int SONIC_DISTANCE_MAX = 450;
const int SONIC_DISTANCE_MIN = 2;

// для управления по блютуз
char btCommand = 'S';
// счетчики для определения потери связи с блютуз
unsigned long btTimer0 = 2000;  //Stores the time (in millis since execution started)
unsigned long btTimer1 = 0;     //Stores the time when the last command was received from the phone
const int in_1 = 22;//управление направления двигателя
const int in_2 = 23;//управление направления двигателя


//ЦАП 0..255
//0В-0
//5В-255
//ЦАП покоя двигателя 0.66В прим. 34
//Минимальная скорость 0.8В прим 41
//Максимальная скорость 2.7В прим 137
void setDAC(byte val){ //функция установления напряжения
   Wire.beginTransmission(PCF8591);         // соединяемся с PCF8591
   Wire.write(0x40);                        // включаем DAC
   Wire.write(val);                         // пишем в DAC значение
   Wire.endTransmission();                  // завершаем соединение
}
void setDAC_2(byte val){ //функция установления напряжения
   Wire.beginTransmission(PCF8591_2);         // соединяемся с PCF8591
   Wire.write(0x40);                        // включаем DAC
   Wire.write(val);                         // пишем в DAC значение
   Wire.endTransmission();                  // завершаем соединение
}


/******************************************
  Main program
******************************************/

void setup() {
  Wire.begin();
  RemoteXY_Init ();
  pinMode (PIN_BUTTON_STOP, OUTPUT);
  Serial.begin(9600);
  pinMode(SONIC_PIN_TRIG, OUTPUT);
  pinMode(SONIC_PIN_ECHO, INPUT);
  pinMode(SONIC_PIN_TRIG_BACK, OUTPUT);
  pinMode(SONIC_PIN_ECHO_BACK, INPUT);
  pinMode(SERVO_DIRECTION_PIN, OUTPUT);
  pinMode(ENCODER_PIN_A, OUTPUT);
  pinMode(ENCODER_PIN_B, OUTPUT);
  pinMode(ENCODER_PIN_C, OUTPUT);
  analogWrite(PWM_PIN_A, PWM_A);
  analogWrite(PWM_PIN_B, PWM_B);
  analogWrite(PWM_PIN_C, PWM_C);
  encoderInit();
  motorInit();
}


void loop() {
  RemoteXY_Handler ();
  if (Serial.available() > 0) {
    btTimer1 = millis();//засекаем время одного цикла
   // ЕСЛИ ТОРМОЗ НЕ НАЖАТ
   if (RemoteXY.button_1==0)
   {
      LeftRight(RemoteXY.joystick_1_x);
      UpDown(RemoteXY.joystick_1_y);
   }
   else
   {
      motorStop();
   }
    btTimer0 = millis();  //Get the current time (millis since execution started).
    //Check if it has been 500ms since we received last btCommand.
    if ((btTimer0 - btTimer1) > 1500)   {
      //если прошло больше 1500мс после последней команды, останавливаем
      motorStop();
    }
  }
}

/******************************************
  Functions
******************************************/
void encoderInit()
{
  SPI.begin();
  digitalWrite(ENCODER_PIN_A, 1);
  digitalWrite(ENCODER_PIN_B, 1);
  digitalWrite(ENCODER_PIN_C, 1);
  SPI.setClockDivider(SPI_CLOCK_DIV32);
}

// инициализация моторов
void motorInit()  {
  setDAC(34);
  motorStop();
}

// безопасно ли ехать вперед?
boolean isSafeDistance(bool z) {
    if ( SAFE_DISTANCE )  {
        // замер расстояния
        if(z==false)
        {
          distance = measureDistanceBefore();
        }
        else if(z==true)
        {
          distance = measureDistanceAfter();
        }
        // препятствие так близко что надо ехать назад ?
        if ( distance < DST_STOP ) {
            return false;
        }
    } else {
        return true;
    }
}
// стоп
void motorStop()  {

}
// Возвращает расстояние до препятствия в сантиметрах
int measureDistanceBefore()  {
  long duration;
  int  distance;
  /* Для запуска передатчика нужно подать на Trig сигнал, длительностью 10мкс.
   * Передатчик который посылает 8 коротких импульсов с частотой 40kHz.
   * Приемник получает отраженный сигнал и на входе Echo генерируется сигнал,
   * длительность которого равна времени прохождения звукового сигнала.
   */
  digitalWrite(SONIC_PIN_TRIG, LOW); // инициализация перед замером
  delayMicroseconds(5); // 3
  digitalWrite(SONIC_PIN_TRIG, HIGH);
  delayMicroseconds(12); // 10
  digitalWrite(SONIC_PIN_TRIG, LOW);

  duration = pulseIn(SONIC_PIN_ECHO, HIGH);
  // Скорость звука 340 м/с или 29 микросекунд на сантиметр.
  // Звук идет вперед и возвращается назад, таким образом время нужно делить на два
  distance = duration / 58; // = microseconds / 29 / 2

  if (distance < SONIC_DISTANCE_MIN )  // out of range
    return SONIC_DISTANCE_MIN;
  if (distance > SONIC_DISTANCE_MAX )  // out of range
    return SONIC_DISTANCE_MAX;

  return distance;
}
int measureDistanceAfter()  {
  long duration;
  int  distance;
  /* Для запуска передатчика нужно подать на Trig сигнал, длительностью 10мкс.
   * Передатчик который посылает 8 коротких импульсов с частотой 40kHz.
   * Приемник получает отраженный сигнал и на входе Echo генерируется сигнал,
   * длительность которого равна времени прохождения звукового сигнала.
   */
  digitalWrite(SONIC_PIN_TRIG_BACK, LOW); // инициализация перед замером
  delayMicroseconds(5); // 3
  digitalWrite(SONIC_PIN_TRIG_BACK, HIGH);
  delayMicroseconds(12); // 10
  digitalWrite(SONIC_PIN_TRIG_BACK, LOW);

  duration = pulseIn(SONIC_PIN_ECHO_BACK, HIGH);
  // Скорость звука 340 м/с или 29 микросекунд на сантиметр.
  // Звук идет вперед и возвращается назад, таким образом время нужно делить на два
  distance = duration / 58; // = microseconds / 29 / 2

  if (distance < SONIC_DISTANCE_MIN )  // out of range
    return SONIC_DISTANCE_MIN;
  if (distance > SONIC_DISTANCE_MAX )  // out of range
    return SONIC_DISTANCE_MAX;

  return distance;
}
void UpDown (int Y)
{
// изменение на джойстике 4..100 = изменение в байтах для ЦАП 41..137 (+37)
if (Y>100) Y=100;
if (Y<-100) Y=-100;
if (Y>4)
{
  if(isSafeDistance(true)==true)
  {
    digitalWrite(in_1,HIGH);
    digitalWrite(in_2,LOW);
    setDAC(Y+37);
    setDAC_2(Y+37);
    delay(1000);//1сек
  }
  else
  {
    motorStop();
  }
}
else if (Y<-4)
{
  if(isSafeDistance(false)==true)
  {
    digitalWrite(in_1,LOW);
    digitalWrite(in_2,HIGH);
    Y*=-1;//берем по модулю
    setDAC_2(Y+37);
    setDAC(Y+37);
    delay(1000);//1сек едет
  }
  else
  {
    motorStop();
  }
}
else
{
  digitalWrite(in_1,LOW);
  digitalWrite(in_2,LOW);
  setDAC(34);// Если джойстик по вертикальной оси не двигается, то состояние покоя 0.66В
}
}

void LeftRight (int X)// 86 71 86
{
  if (X>100) X=100;
  if (X<-100) X=-100;
  if (X>4)
  {
    Right(ENCODER_PIN_A, PWM_PIN_A, 86, X);
    Right(ENCODER_PIN_B, PWM_PIN_B, 71, X);
    Right(ENCODER_PIN_C, PWM_PIN_C, 86, X);
    delay(10);
  }
  else if(X<-4)
  {
    X*=-1;
    Left(ENCODER_PIN_A, PWM_PIN_A, 86, X);
    Left(ENCODER_PIN_B, PWM_PIN_B, 71, X);
    Left(ENCODER_PIN_C, PWM_PIN_C, 86, X);
    delay(10);
  }
  else
  {//при отпускании руля нужно вычислить значение угла, чтобы привести его к нулю
    ToNull(ENCODER_PIN_A, PWM_PIN_A, PWM_A);
    ToNull(ENCODER_PIN_B, PWM_PIN_A, PWM_B);
    ToNull(ENCODER_PIN_C, PWM_PIN_A, PWM_C);
  }
}

void ToNull(int ENCODER_PIN, int PWM_PIN, int PWM)
{
    digitalWrite(ENCODER_PIN, 0);
    byte a = SPI.transfer(0);
    byte b = SPI.transfer(1);
    digitalWrite(ENCODER_PIN, 1);
    int angle = (a << 8) | b;
    while(angle!=0)
    {

      if ((angle<512)&&(angle>10))
      {
        digitalWrite(SERVO_DIRECTION_PIN, 0);//обратное направлние
        analogWrite(PWM_PIN, PWM);
        delay(10);
      }
      else if ((angle>512)&&(angle<1013))
      {
        digitalWrite(SERVO_DIRECTION_PIN, 1);
        analogWrite(PWM_PIN, PWM);
        delay(10);
      }

      else if((angle<512)&&(angle<10))//ДЛЯ ИЗБЕЖАНИЯ ЛИШНИХ КОЛЕБАНИЙ КОЛЕС В ЛЕВО ВПРАВО ПРИ МАЛЕНЬКИХ УГЛАХ
      {
        digitalWrite(SERVO_DIRECTION_PIN, 0);
        analogWrite(PWM_PIN, 100);
      }
      else //ДЛЯ ИЗБЕЖАНИЯ ЛИШНИХ КОЛЕБАНИЙ КОЛЕС В ЛЕВО ВПРАВО ПРИ МАЛЕНЬКИХ УГЛАХ
      {
        digitalWrite(SERVO_DIRECTION_PIN, 1);
        analogWrite(PWM_PIN, 100);
      }
      delay(2);
    }
    analogWrite(PWM_PIN, 0);
}
void Right(int ENCODER_PIN, int PWM_PIN, int MAXANGLE, int X)
{
    digitalWrite(SERVO_DIRECTION_PIN, 0);
    digitalWrite(ENCODER_PIN, 0);
    byte a = SPI.transfer(0);
    byte b = SPI.transfer(1);
    digitalWrite(ENCODER_PIN, 1);
    int angle = (a << 8) | b;//склейка из двух байтов числа int от 0 до 1023
    if(X*MAXANGLE/100<=angle)
    analogWrite(PWM_PIN, 0);
}
void Left(int ENCODER_PIN, int PWM_PIN, int MAXANGLE, int X)
{
    digitalWrite(SERVO_DIRECTION_PIN, 1);
    digitalWrite(ENCODER_PIN, 0);
    byte a = SPI.transfer(0);
    byte b = SPI.transfer(1);
    digitalWrite(ENCODER_PIN, 1);
    int angle =1023- (a << 8) | b;//отсчет в обратную сторону
    if(X*MAXANGLE/100<=angle)
    analogWrite(PWM_PIN, 0);
}