#include <Servo.h>
Servo myservo1;
Servo myservo2;
#include "GoBLE.h"
int buttonState[7];
#include "Metro.h"

void setup () {
Serial.begin(115200);
myservo1.attach(3);  //x
myservo2.attach(10);  //top
Goble.begin();
}
void loop () {
  if (Goble.available()) {
    buttonState[SWITCH_UP]     = Goble.readSwitchUp();
    buttonState[SWITCH_DOWN]   = Goble.readSwitchDown();
    buttonState[SWITCH_LEFT]   = Goble.readSwitchLeft();
    buttonState[SWITCH_RIGHT]  = Goble.readSwitchRight();
  //buttonState[SWITCH_SELECT] = Goble.readSwitchSelect();
  //buttonState[SWITCH_START]  = Goble.readSwitchStart();
   
  for (int i = 1; i < 7; i++) {                           //注释a
    static float currentServoTop = 90;
    static float currentServoBottom = 90;   
  //Serial.print("Button ID: ");
  //Serial.print(i);
  //Serial.print("\t State: ");
       if (buttonState[SWITCH_UP]) { //一号键位
        currentServoTop  = clip(0, 180, currentServoTop - 1);
      }
      if (buttonState[SWITCH_DOWN]) { //三号键位
        currentServoTop  = clip(0, 180, currentServoTop + 1);
      }
     if (buttonState[SWITCH_LEFT]) { //四号键位
        currentServoBottom  = clip(60, 150, currentServoBottom - 1);
      }
      if (buttonState[SWITCH_RIGHT]) {  //二号键位
        currentServoBottom  = clip(60, 150, currentServoBottom + 1);
      }
      myservo1.write(int(currentServoBottom));
      myservo2.write(int(currentServoTop));    
    }
  }
}
int clip(float min, float max, float Value)         //注释b
{
  if (Value < min) {
    return min;
  }
  else if (Value > max)
  {
    return max;
  }
  return Value;
}