/* This is the Code for a hand controlled robotic arm for a 
warhammer 40k Tech-Priest costume, it takes inputs from the accelerometer and converts
them into servo movements.
*/

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>
#include <Wire.h>

// variable declarations
int angle1;
int angle2;
int angle3;
int angle4;
int angle5;
string mainswitch;



Adafruit_MPU6050 mpu;

Servo servo1; // base rotation 
Servo servo2; // forward joint
Servo servo3; // forward + up/down joint
Servo servo4; // claw rotation
Servo servo5; // grabber piece


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

  pinMode(8, INPUT);

  while (!mpu.begin()) { // ensure mpu6050 is connected before starting code
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  //assign servos to pins
  servo1.attach(11);
  servo2.attach(10);
  servo3.attach(9);
  servo4.attach(6);
  servo5.attach(5);

  //set the starting positions of the servos
  angle1 = 90;
  angle2 = 0;
  angle3 = 180;
  angle4 = 90;
  angle5 = 0;
}

sensors_event_t event;

void loop() {
  
  // get the x y z values from the accelerometer in the mpu6050
  mpu.getAccelerometerSensor()->getEvent(&event);

  mainswitch = digitalRead(8);
  Serial.write(mainswitch);

  //if the first switch is on, update the servo positions
  if(mainswitch == HIGH);
    {
    angle1 = min(angle1, 180);
    angle1 = max(angle1, 0);
    angle1 =(angle1 + (event.acceleration.x));
    servo1.write(angle1);
    delay(10);

    angle2 = min(angle2, 180);
    angle2 = max(angle2, 0);
    angle2 =(angle2 + (event.acceleration.z) - 1);
    servo2.write(angle2);
    delay(10);

    angle3 = min(angle3, 180);
    angle3 = max(angle3, 0);
    angle3 =(angle3 + (event.acceleration.y));
    servo3.write(angle3);
    delay(10);
    
    //get the x y z rotational values from the mpu6050
    mpu.getGyroSensor()->getEvent(&event);

    angle4 = min(angle4, 180);
    angle4 = max(angle4, 0);
    angle4 =(angle4 + ((event.gyro.z)*2));
    servo4.write(angle4);
    delay(10);
    }

  if (digitalRead(4) == HIGH) {
    servo5.write(180);

    } else {
     servo5.write(0);

    }
}