 */
 #include <Servo.h>
 
-
-const int STEER_IN = 5;
-const int STEER_OUT = 9;
-const int STEER_BASE = 1277;
-const int STEER_MIN = 847;
-const int STEER_MAX = 1710;
-const float STEER_SENSITIVITY = 1.0;
-
-const int THROTTLE_IN = 6;
-const int THROTTLE_OUT = 11;
-const int THROTTLE_BASE = 1273;
-const int THROTTLE_MIN = 843;
-const int THROTTLE_MAX = 1710;
-const float THROTTLE_SENSITIVITY = 1.0;
-
-const int LED_GREEN = 4;
-const int LED_RED = 3;
-
-const int RPM_INTERRUPT = 0; //pin 2
+#define STEER_IN 5
+#define STEER_OUT 9
+#define STEER_BASE 1277
+#define STEER_MIN 847
+#define STEER_MAX 1710
+#define STEER_SENSITIVITY 1.2
+
+#define THROTTLE_IN 6
+#define THROTTLE_OUT 11
+#define THROTTLE_BASE 1273
+#define THROTTLE_MIN 843
+#define THROTTLE_MAX 1710
+#define THROTTLE_SENSITIVITY 3.0
+
+#define LED_GREEN 4
+#define LED_RED 3
+
+#define RPM_PIN 2
+#define RPM_INTERRUPT 0
+
+// left and right sonar trigger at the same time
+#define TRIGGER_PIN     7   
+#define ECHO_PIN_LEFT   8
+#define ECHO_PIN_RIGHT  12
 
 int _STEER_BIAS = 0; //set this to adjust steering
-int _GOVERNER = 1710; //cap the forward speed
+int _GOVERNER_F = 10; //cap the forward speed
+int _GOVERNER_R = -12;
 
 
 //servos

  Serial.begin(115200);
  Serial.println("V79: starting...");
  attachInterrupt(RPM_INTERRUPT, monitor_rpm, RISING);

  //set up red/green led indicators
  pinMode(LED_GREEN, OUTPUT);

void set_throttle_position(int pos) {
  //dont set unless a delta has been acheived
  if (abs(throttle_pos - pos)/2.0 < THROTTLE_SENSITIVITY){
    if (abs(pos) <= 5){
      pos = 0;
    }
    else {
      //not a big of change to change noise
      return;
    }

    //if this value has already been set zero, no reason to set it again
    if (throttle_pos == 0) {
      return;
    }
  }
  

  //determine if in reverse
  if (pos > 0){
    in_reverse = false;
  }
  else if (throttle_pos == 0){
    //only go in reverse if the throttle was zero
    in_reverse = true;
  }


  //dont allow input past the limits, note we only limit reverse if the 
  //standing throttle position is at zero. This allows for full breaking
  if (in_reverse){
    pos = max(_GOVERNER_R, pos);
  }
  
  pos = min(_GOVERNER_F, pos);

  throttle_pos = pos;
  Serial.println(String("V79-T:") + throttle_pos);

  //note!!!!! the 1000 and 2000 here are the min and max PWM that the controller accepts
  int map_throttle = map(pos, -100, 100, 1000, 2000);
  

  throttle_servo.writeMicroseconds(map_throttle);
}
