#include <Servo.h> 
 
Servo drive;
 
int drive_pin = 9;
int orient_pin = 0;
int orient = 0;
int want_orient = 0;
int diff = 0;
int strength = 0;
int slower = 0;
int ledPin = 13;
 
void setup() 
{ 
  drive.attach(drive_pin);
  pinMode(ledPin, OUTPUT);   
} 
 
void loop() 
{ 
  want_orient = want_orient + 1;
  
  if(want_orient > 1023) { want_orient = want_orient - 1024; }
  
  
  orient = analogRead(orient_pin);
  
  want_orient = 100;
  
  if(orient - want_orient > 50) {
    digitalWrite(ledPin, HIGH);
    strength = strength +1; 
  } else if (orient - want_orient < -50) {
    digitalWrite(ledPin, HIGH);
    strength = strength -1;
  } else {
    digitalWrite(ledPin, LOW);
  }
  strength = constrain(strength, -40, 40);
  drive.write(91 + strength/10);

  /*
  //diff = want_orient - orient;
  //if(abs(diff) > 511) diff = (diff/abs(diff))*(1024-diff);
  diff = 0;
  
  
  
  diff = diff / (511/89);
  digitalWrite(ledPin, LOW);
  if(abs(diff) < 20) { strength = 90; digitalWrite(ledPin, HIGH); }
  else if (abs(diff) > 90) strength = (diff/abs(diff)) * 89 + 90;
  else strength = 90 + diff;
  
  if(abs(want_orient - orient) < 50) digitalWrite(ledPin, HIGH);

  drive.write(strength);
    */
  delay(15);
}