//Flex Sensor
#include <Servo.h>
#include <AccelStepper.h>

//MPU 6050
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
Servo servoMotor_Y;
Servo servoMotor_X;

int16_t ax, ay, az, gx, gy, gz;
int servo_Y = 5;  // Choose the pin where the servo is connected
int servo_X = 6;

//stepper
#define STEP_PIN 13		//Stepper PIN
#define DIR_PIN 12		//Stepper PIN
#define POT_PIN A0	//Flex Sensor

AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);  // create class instance 


Servo Sholder;  // create servo object to control a servo

int potpin1 = A1;  // analog pin used to connect the potentiometer
int val_1;

Servo Wrist_1;  // create servo object to control a servo

int potpin2 = A2;  // analog pin used to connect the potentiometer
int val_2;

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

  //Flex Sensor
  Sholder.attach(3);
  Wrist_1.attach(4);

  //MPU6050
  Wire.begin();
  mpu.initialize();
  
  servoMotor_Y.attach(servo_Y);
  servoMotor_X.attach(servo_X);

  //Stepper motor
  stepper.setMaxSpeed(1000.0);		//Set motor max speed
  stepper.setAcceleration(500.0);	//Set acceleration

}

void loop() 
{
// Flex Sensor
  val_1 = analogRead(potpin1);
  val_1 = map(val_1, 0, 1023, 0, 180);
  Sholder.write(val_1);
  delay(15);

  val_2 = analogRead(potpin2);
  val_2 = map(val_2, 0, 1023, 0, 180);
  Wrist_1.write(val_2);
  delay(15);

//MPU6050 
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Map the Y-axis rotation to servo angle range (0-180)
  int servoAngle_Y = map(gy, -32768, 32767, 0, 180);
  int servoAngle_X = map(gx, -32768, 32767, 0, 180);
  
  // Move the servo motor based on Y-axis rotation
  servoMotor_Y.write(servoAngle_Y);
  servoMotor_X.write(servoAngle_X);

  Serial.print("Servo Angle for Y: ");
  Serial.println(servoAngle_Y);

  Serial.print("Servo Angle for X: ");
  Serial.println(servoAngle_X);
  //delay(100); // Adjust delay as needed

//stepper
  int potValue = analogRead(POT_PIN);

  // Map the potentiometer value to the stepper motor speed
  int speed = map(potValue, 0, 1023, 0, 1000);

  // Print potentiometer value and control the stepper motor
  Serial.print("Potentiometer Value: ");
  Serial.println(potValue);

  // Check the direction of rotation based on the potentiometer value
  if (potValue >= 0 && potValue <= 400) 
  {
    // Move stepper motor with positive speed
    stepper.setSpeed(speed);
    stepper.move(50);
  } 
  else if (potValue > 400 && potValue <= 600) 
  {
    // Motor stays at rest
    stepper.setSpeed(0);
  } 
  else if (potValue > 600 && potValue <= 1023) 
  {
    // Move stepper motor with negative speed
    stepper.setSpeed(-speed);
    stepper.move(-50);
  }

  // Run the stepper motor
  stepper.run();
}