// Варіант                               12
// Кут повороту сервопривода             ±38º
// Режими перемикання керування колесами 1,2,3,4

#include <Servo.h>
#include "U8glib.h"

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_FAST);

#define FIRST_SERVO_PIN 2
#define POTENTIOMETER_PIN A6
#define BUTTON_PIN 6

#define ANGLE_ROTATE 38

Servo servos[4];

byte servoDeg[4] = {90, 90, 90, 90};
char servoName[4][5] = {"UpL:","UpR:", "DnL:", "DnR:"};

byte currentServo = 0;

void setup() {
  for (byte i = 0; i < 4; i++) {
    servos[i].attach(i + FIRST_SERVO_PIN);
    servos[i].write(90);
  }

  pinMode(POTENTIOMETER_PIN, INPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  u8g.setFont(u8g_font_tpssb);
  u8g.setColorIndex(1);
}

void loop() {
  byte newDeg = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 
                    90 - ANGLE_ROTATE, 90 + ANGLE_ROTATE + 1);

  if (digitalRead(BUTTON_PIN) == LOW) {
    currentServo ++;
    if (currentServo >= 4) currentServo = 0;
    drawAndRotate();
    delay(100);
  }

  if (servoDeg[currentServo] != newDeg){
    servoDeg[currentServo] = newDeg;
    drawAndRotate();
    delay(20);
  }

}


void drawAndRotate(){
  servos[currentServo].write( servoDeg[currentServo] );
  u8g.firstPage();
  do {
    byte servoIndex = 0;
    for(byte y=0; y<2; y++){
      for(byte x=0; x<2; x++){
        char drawDeg[3]; itoa(servoDeg[servoIndex], drawDeg, 10);
        if(currentServo == servoIndex) {
        u8g.drawStr(x*64,       y*32+10, ">");
        u8g.drawStr(x*64,       y*32+25, ">"); }
        u8g.drawStr(x*64+20,    y*32+10, servoName[servoIndex]);
        u8g.drawStr(x*64+10,    y*32+25, drawDeg);
        u8g.drawStr(x*64+10+25, y*32+25, "deg");
        servoIndex++;
      }
    }
  } while (u8g.nextPage());
}