/*/   Mobile Platform for Tele-operated Robotic Arm   /*/
// Code by Michael Fernandez, Nov 2024


#include <Servo.h>
#include <ESP8266WiFi.h>
#include <espnow.h>


// Driver pin
const uint8_t motorA = 14;  // D5
const uint8_t motorB = 16;  // D0
const uint8_t in1 = 2;      // D4
const uint8_t in2 = 0;      // D3
const uint8_t in3 = 4;      // D2
const uint8_t in4 = 5;      // D1


// Joystick variable
uint16_t joyXvalue = 533;
uint16_t joyYvalue = 533;
bool gyroCtrl = false;


// Servo
const uint8_t clawServoPin = 15;  // D8
const uint8_t pitchServoPin = 13; // D7
const uint8_t yawServoPin = 12;   // D6
bool clawBtn1 = false;
bool clawBtn2 = false;
uint8_t clawServoAngle = 90;

Servo pitchServo, yawServo, clawServo; 

uint16_t pitchServoAngle;
uint16_t yawServoAngle;


// Data Receiver
typedef struct struct_message {
  uint16_t joyXdata;
  uint16_t joyYdata;
  bool gyroCtrlData;
  float gyroZdata;
  float gyroYdata;
  bool clawBtn1Data;
  bool clawBtn2Data;
} struct_message;

struct_message transferData;


// Callback function that will be executed when data is received
void OnDataRecv(uint8_t * mac, uint8_t *incomingData, uint8_t len) {
  memcpy(&transferData, incomingData, sizeof(transferData));
  joyXvalue = transferData.joyXdata;

  Serial.print("joystick x data: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(joyXvalue);

  joyYvalue = transferData.joyYdata;

  Serial.print("joystick y data: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(joyYvalue);

  gyroCtrl = transferData.gyroCtrlData;
  
  Serial.print("gyro on/off: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(gyroCtrl);

  clawBtn1 = transferData.clawBtn1Data;

  Serial.print("claw button 1: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(clawBtn1);

  clawBtn2 = transferData.clawBtn2Data;

  Serial.print("claw button 2: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(clawBtn2);

  pitchServoAngle = transferData.gyroZdata;

  Serial.print("gyro Z: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(pitchServoAngle);

  yawServoAngle = transferData.gyroYdata;

  Serial.print("gyro Y: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(yawServoAngle);
}


void checkClawBtn() {
  static uint8_t loopCount = 0;
  static bool timeout = false;

  Serial.println("claw servo angle= "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(clawServoAngle);

  if (clawBtn1 == HIGH && clawBtn2 == LOW && clawServoAngle > 0) {
    clawServoAngle -= 5;
    clawServo.attach(clawServoPin);
    clawServo.write(clawServoAngle);
    timeout = true;
    loopCount = 0;
    Serial.println("claw opening"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  }else if (clawBtn1 == LOW && clawBtn2 == HIGH && clawServoAngle < 90) {
    clawServoAngle += 5;
    clawServo.attach(clawServoPin);
    clawServo.write(clawServoAngle);
    timeout = true;
    loopCount = 0;
    Serial.println("claw closing"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  }

  if (timeout == true && loopCount >= 100) {
    clawServo.detach();
    timeout = false;
    loopCount = 0;
    Serial.println("claw detached"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  }else if (timeout == true) {
    loopCount++;
  }
}

void AB_MtrForwad() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);  
}
void AB_MtrBackward() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void AB_MtrRight() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}
void AB_MtrLeft() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void motorLogic() {
  static bool xNeutral;
  static bool yNeutral;
  const uint16_t maxNeutral = 540;
  const uint16_t minNeutral = 525;

  if (joyXvalue >= minNeutral - 50 && joyXvalue <= maxNeutral + 50) {
    xNeutral = true;
  } else {
    xNeutral = false;
  }
  if (joyYvalue >= minNeutral - 50 && joyYvalue <= maxNeutral + 50) {
    yNeutral = true;
  } else {
    yNeutral = false;
  }

  Serial.print("motor go= "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  
  // Turn off motor joystick neutral
  if (xNeutral && yNeutral) {
    digitalWrite(motorA, LOW);
    digitalWrite(motorB, LOW);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
    Serial.print("stopped"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  }

  // Go Forward or turn right/left
  else if (joyYvalue <= minNeutral && xNeutral) {  // Straight forward
    AB_MtrForwad();
    uint8_t motorSpeed = map(joyYvalue, minNeutral, 0, 125, 255);
    analogWrite(motorA, motorSpeed);
    analogWrite(motorB, motorSpeed);

    Serial.print("forward"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  } else if (joyYvalue <= minNeutral) {  // Turning right forward
    AB_MtrForwad();
    uint8_t motorSpeed = map(joyYvalue, minNeutral, 0, 125, 255);
    if (joyXvalue <= minNeutral) {
      uint8_t motorGoRight = map(joyXvalue, minNeutral, 0, 125, 255);
      int8_t totalSpeed = motorSpeed - motorGoRight;
      uint8_t aSpeed = constrain(totalSpeed, 125, 255);
      
      analogWrite(motorA, aSpeed);
      analogWrite(motorB, motorSpeed);

      Serial.print("front right"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    }else {
      uint8_t motorGoLeft = map(joyXvalue, maxNeutral, 1024, 125, 255);
      int8_t totalSpeed = motorSpeed - motorGoLeft;
      uint8_t bSpeed = constrain(totalSpeed, 125, 255);
      
      analogWrite(motorA, motorSpeed);
      analogWrite(motorB, bSpeed);

      Serial.print("front left"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    }
  }

  // Go bacward or turn left/right
  else if (joyYvalue >= maxNeutral && xNeutral){  // Straight backward
    AB_MtrBackward();
    uint8_t motorSpeed = map(joyYvalue, maxNeutral, 1024, 125, 255);
    analogWrite(motorA, motorSpeed);
    analogWrite(motorB, motorSpeed);

    Serial.print("backward"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  } else if (joyYvalue >= maxNeutral) { // Turn rigt backward
    AB_MtrBackward();
    uint8_t motorSpeed = map(joyYvalue, maxNeutral, 1024, 125, 255);
    if (joyXvalue <= minNeutral) {
      uint8_t motorGoRight = map(joyXvalue, minNeutral, 0, 125, 255);
      int16_t totalSpeed = motorSpeed - motorGoRight;
      uint8_t aSpeed = constrain(totalSpeed, 125, 255);
      
      analogWrite(motorA, aSpeed);
      analogWrite(motorB, motorSpeed);

      Serial.print("back left"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    }else {
      uint8_t motorGoLeft = map(joyXvalue, maxNeutral, 1024, 125, 255);
      int8_t totalSpeed = motorSpeed - motorGoLeft;
      uint8_t bSpeed = constrain(totalSpeed, 125, 255);
      
      analogWrite(motorA, motorSpeed);
      analogWrite(motorB, bSpeed);

      Serial.print("back right"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    }
  } 

  // Go Right or left (Neutral steering)
  else if (yNeutral && joyXvalue <= minNeutral) {  // Right neutral steering
    AB_MtrRight();
    uint8_t motorSpeed = map(joyXvalue, minNeutral, 0, 125, 200);
    analogWrite(motorA, motorSpeed);
    analogWrite(motorB, motorSpeed);

    Serial.print("neutral steer right"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  }else if (yNeutral && joyXvalue >= maxNeutral) { // Left neutral steering
    AB_MtrLeft();
    uint8_t motorSpeed = map(joyXvalue, maxNeutral, 1024, 125, 200);
    analogWrite(motorA, motorSpeed);
    analogWrite(motorB, motorSpeed);

    Serial.print("neutral steer left"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  }
  Serial.println(" "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
}


void setup() {
  Serial.begin(115200);

  pinMode(motorA, OUTPUT);
  pinMode(motorB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  
  clawServo.attach(clawServoPin);
  pitchServo.attach(pitchServoPin);
  yawServo.attach(yawServoPin);
  clawServo.write(clawServoAngle);
  pitchServo.write(90);
  yawServo.write(90);
  delay(500);
  pitchServo.detach();
  yawServo.detach();

  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != 0) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info
  esp_now_set_self_role(ESP_NOW_ROLE_SLAVE);
  esp_now_register_recv_cb(OnDataRecv);
}

void loop() {
  Serial.println(" "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  motorLogic();

  if (gyroCtrl) {
    checkClawBtn();

    pitchServo.attach(pitchServoPin);
    yawServo.attach(yawServoPin);
    uint8_t fltredPitchAngle = constrain(pitchServoAngle, 25, 180);
    pitchServo.write(fltredPitchAngle);
    yawServo.write(yawServoAngle);

    Serial.print("pitch= "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(pitchServoAngle);
    Serial.print("yaw= ");
    Serial.println(pitchServoAngle);
  } else {
    pitchServo.detach();
    yawServo.detach();

    static uint8_t resetLoopCount = 0;

    static uint8_t resetTest = 0; //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

    switch (resetLoopCount) {
      case 0:
        if (clawBtn1 == HIGH && clawBtn2 == HIGH) {
          resetLoopCount++;
        }
        break;
      case 255:
        pitchServo.attach(pitchServoPin);
        yawServo.attach(yawServoPin);
        if (lastPitchServoAngle > 90) {
          lastPitchServoAngle --;
          pitchServo.write(lastPitchServoAngle);
          Serial.println("up");//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        }else if (lastPitchServoAngle < 90) {
          lastPitchServoAngle ++;
          pitchServo.write(lastPitchServoAngle);
          Serial.println("down");//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        }

        if (lastYawServoAngle > 90) {
          lastYawServoAngle --;
          yawServo.write(lastYawServoAngle);
          Serial.println("right");//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        }else if (lastYawServoAngle < 90) {
          lastYawServoAngle ++;
          yawServo.write(lastYawServoAngle);
          Serial.println("left");//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        }

        if (lastYawServoAngle == 90 && lastPitchServoAngle == 90) {
          resetLoopCount = 0;
          pitchServo.detach();
          yawServo.detach();
        }
        break;
      default:
        if (clawBtn1 == HIGH && clawBtn2 == HIGH && resetLoopCount < 100) {
          resetLoopCount++;
        }else if (clawBtn1 == LOW && clawBtn2 == LOW) {
          resetLoopCount = 0;
        }else {
          resetLoopCount = 255;
          lastPitchServoAngle = pitchServoAngle;
          lastYawServoAngle = yawServoAngle;
          resetTest++; //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        }
        break;
    }
    Serial.print("loopcount= ");//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(resetLoopCount);
    Serial.print("pitch servo= ");
    Serial.println(pitchServoAngle);
    Serial.print("yaw servo= ");
    Serial.println(yawServoAngle);
    Serial.print("reset arm angle total= "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(resetTest);
  }

  delay(10);
}