// Варіант                               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];

int servoDeg[4] = {90, 90, 90, 90};

// 1	Повертаються тільки передні колеса
// 2	Повертаються передні та задні колеса в протилежні сторони
// 3	Повертаються передні та задні колеса в одну сторону
// 4	Повертаються передні та задні колеса в протилежні сторони (розворот на місці)
// Режими де сервоприводи 1234
char servoName[4][5] = {"++00","++--", "++++", "+--+"};

byte currentServoMode = 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() {
  int newDeg = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 
                      -ANGLE_ROTATE, ANGLE_ROTATE+1);

  if (! digitalRead(BUTTON_PIN)) {
    currentServoMode ++;
    if (currentServoMode >= 4) currentServoMode = 0;
    servoDeg[currentServoMode] = newDeg;
    drawAndRotate();
    delay(300);
  }

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

}

int getAngle(byte i){
  char mode = servoName[currentServoMode][i];
  if (mode == '0') return 90;
  return 90 + servoDeg[currentServoMode] * ((mode == '+') ? 1 : -1);  
}

void drawAndRotate(){
  for(byte i=0; i<4; i++) servos[i].write(getAngle(i));  

  u8g.firstPage();
  do {
    byte servoIndex = 0;
    for(byte y=0; y<2; y++){
      for(byte x=0; x<2; x++){
        char drawDeg[3]; itoa(getAngle(servoIndex), drawDeg, 10);
        if(currentServoMode == servoIndex)     {
        u8g.drawStr(x*64,       y*32+10, ">"); }
        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());
}