#include <Servo.h>

// Leonardo Digital Pins:  4,7,8,9,12,13
//          Analog Pins:   A0 - A5   // use analogRead()  or  analogWrite()

#define JOY_BACKBONE_PIN A0 
#define JOY_FOREBACK_PIN A4   
#define JOY_LEFTRIGHT_PIN A5  

#define BACKBONE_OUT_PIN  8   // must support PWM 9
#define RIGHT_MOTOR_PIN   4   // must support PWM  3
#define LEFT_MOTOR_PIN   12   // must support PWM  6
#define BUZZER_PIN        2

Servo myservo;  // create servo object to control a servo

int backbone_pos=90;  // retains backbone position
int backbone_move=0;
int toggle=0;

void setup() 
{
// SETUP INPUTS
  pinMode(JOY_BACKBONE_PIN, INPUT);
  pinMode(JOY_FOREBACK_PIN, INPUT);
  pinMode(JOY_LEFTRIGHT_PIN, INPUT);

// SETUP OUTPUTS
  myservo.attach(BACKBONE_OUT_PIN);  // attach the servo on pin 9 to the servo object
  pinMode(RIGHT_MOTOR_PIN, OUTPUT);  // right motor PWM signal
  pinMode(LEFT_MOTOR_PIN, OUTPUT);   // left motor PWM signal
  pinMode(BUZZER_PIN, OUTPUT);

pinMode(LED_BUILTIN, OUTPUT);

  myservo.write(backbone_pos);       // send initial position to servo
}

void loop() 
{
  int backbone_joy = analogRead(JOY_BACKBONE_PIN);  // reads backbone joystick (value between 0 and 1023)
  if (backbone_joy < 500) 
    backbone_move = backbone_pos > 0 ? -10 : 0;
  else if (backbone_joy > 524)
    backbone_move = backbone_pos <180 ? 10 : 0;

  if (backbone_move != 0)
  {
    backbone_pos += backbone_move;
    backbone_move = 0;
    myservo.write(backbone_pos);       // sets the backbone servo position according to the scaled value
  }

  int foreback = analogRead(JOY_FOREBACK_PIN);   // read vert goes from 0 (bottom) to 1023 (top)
  int leftright = analogRead(JOY_LEFTRIGHT_PIN);   // read steering position - horz goes from 0 (right) to 1023 (left)
  if (foreback>524)                             // going FORWARD?
  {
    int speed = map(foreback, 525, 1023, 10, 255);  // scale the joystick force to the motor speed range 
    analogWrite(RIGHT_MOTOR_PIN, speed);      // turn on both motors
    analogWrite(LEFT_MOTOR_PIN, speed);

    //tone(BUZZER_PIN, speed*30, 50);  // Plays a speed-proportional tone for 50 ms
  }
  else if (leftright<500)                   // going LEFT?
  {
    int speed = map(leftright, 500, 0, 10, 255);  // scale the joystick force to the motor speed range 
    analogWrite(LEFT_MOTOR_PIN, speed);       // turn on left motor
  }
  else if (leftright>524)                   // going RIGHT?
  {
    int speed = map(leftright, 525, 1023, 10, 255);  // scale the joystick force to the motor speed range 
    analogWrite(RIGHT_MOTOR_PIN, speed);      // turn on right motor
  }
  else 
  {
    analogWrite(LEFT_MOTOR_PIN, 0);         // turn off both motors
    analogWrite(RIGHT_MOTOR_PIN, 0);
  }

  digitalWrite(LED_BUILTIN, (toggle<10) ? HIGH : LOW);   // turn the LED on (HIGH is the voltage level)
  if (toggle>=20)
    toggle = 0;
  else
    toggle++;
  delay(50);   // pause to let allow inputs to happen
}