 void setup() {
   Serial.begin(9600);
   pinMode(buttonPin, INPUT);
+  rightArmData = "fwd90";
 }
 
 //Run loop continuously
 void loop() {
-  String content = "";
-  char character;
-
-  while(Serial.available()) {
-    character = Serial.read();
-    content.concat(character);
-    updated = true;
-  }
-
-  if (updated) {
-    parseJSON(content);
-  }
+  Serial.print(armFwdRight90());
+  Serial.print(" | ");
+  Serial.print(analogRead(rShoulderPin));
+  Serial.print(" | ");
+  Serial.print(armFR90ShoulderVal);
+  Serial.print(" | ");
+  Serial.print(analogRead(rUnderarmPin));
+  Serial.print(" | ");
+  Serial.print(armFR90UnderVal);
+  Serial.print(" | ");
   
   checkPosition();
   delay(500);
 }
 
-//Parse json string
-void parseJSON(String content) {
-  updated = false;
-  JsonObject& in = jsonBuffer.parseObject(const_cast<char*>(content.c_str()));
-  
-  name = in["name"];
-  leftArmData = in["leftArm"];
-  rightArmData = in["rightArm"];
-  backData = in["back"];
-
-  if (!in.success()) {
-    Serial.println("JSON parsing failed");
-    return;
-  }
-  oldJSON= "";
-  checkPosition();
-}
 
 //Check if the position from json data is right
 void checkPosition() {

//Buffer for json parsing
//Could potentially take up too much memory
DynamicJsonBuffer jsonBuffer;
bool updated = false;

//For smoothing of data
bool prevBackData[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
bool prevLeftData[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
bool prevRightData[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int counter = 0;


  } else {
    rightArm = false;
  }
  
  Serial.print(" - ");
  Serial.println(rightArm);
}
