#include "globals.h"

int gasVal = 0, servoVal = 0;
unsigned long msAuto = 0;

Servo myServo;
//ESC myGas(GAS_PIN, 1000, 2000, 500);
Servo myGas;

MPU6050 mpu;
Spm4648 spm;

void initServos() {
  myServo.attach(SERVO_PIN);
  myGas.attach(GAS_PIN);
  //myGas.calib();
  //myGas.arm();
  setServo(0);
  setMotor(0);
}

void setMotor(float val) {
  val = constrain(val, -1, 1);
  //gasVal = (val + 1.5) * 500; // 1000 .. 2000
  gasVal = (val + 1) * 90;
  myGas.write(gasVal);
}
void setServo(float val) {
  val = constrain(val, -1, 1);
  servoVal = 180 - (val + 0.08 + 1) * 90; // 180 .. 0
  myServo.write(servoVal);
}