#include <EEPROM.h>
#include "Robot.h"

int data[3] = {0,275,35}; 
int data2[3] = {0,0,0};

void setup() {
  
  Serial.begin(115200);
  while(!Serial);
  delay(500);

  // for(int i = 0; i < )
  
  Robot robot;
  robot.savePositions(data);
  robot.getPositions(data2);

  for(int i = 0; i < 3; i++){
    Serial.println(data2[i]);
  }
  

}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()){
    String msg = Serial.readString();
    msg = msg.substring(0,msg.length()-1);
    String out = "";

    if(msg == "shoulder")
      out = "AlterShoulder";
    else if(msg == "elbow")
      out = "AlterElbow";
    else if(msg == "base")
      out = "AlterBase";
    else 
      out = "Command Not Recognized";

    Serial.println(out);
    
  }
}