 
 
 double PIDThrottleOutput; //used to tell Throttle and Brake what to do as far as acceleration
-double desiredSpeed;
-
-double proportionalConstant = 5;
-double integralConstant = 5;
-double derivativeConstant = 1;
+double desiredSpeed = 2800.0; //aprox 10kph
+#define PID_CALCULATE_TIME 50
+
+double proportionalConstant = .0175;
+double integralConstant = .0141;
+double derivativeConstant = .00001;
 
 // PID setup block
 PID speedPID(&SpeedCyclometer_mmPs, &PIDThrottleOutput, &desiredSpeed, proportionalConstant, integralConstant, derivativeConstant, DIRECT);
-//speedPID.SetSampleTime(int time); //useful if we want to change the compute period
+
 
 
 //...................................................................................
 
 void setup(){
-  Serial.begin(9600);  
+  Serial.begin(9600);
+  SPI.begin(); 
   speedPID.SetOutputLimits(MIN_ACC_OUT, MAX_ACC_OUT); //useful if we want to change the limits on what values the output can be set to
+  speedPID.SetSampleTime(PID_CALCULATE_TIME); //useful if we want to change the compute period
   setupWheelRev();
   //attachInterrupt(digitalPinToInterrupt(IRPT_RDR), ISR_RDR_rise, RISING);
+  STEER_SERVO.attach(STEER_OUT_PIN);
+  STEER_SERVO.writeMicroseconds(STRAIGHT_TURN_OUT);
 }
 
 void loop(){

    data->nowTime_ms = TickTime;
    data->oldClickNumber = data->nowClickNumber;
    data->nowClickNumber = ClickNumber;
  
}


//  Serial.println(InterruptState);
//  Serial.print("Click ");
//  Serial.println(ClickNumber);
  //delay(50); 
}

void ThrottlePID(){
  
  speedPID.Compute();

  Serial.print("Throttle out value ");
  Serial.println(PIDThrottleOutput);
  int throttleControl = (int)PIDThrottleOutput;
  moveVehicle(throttleControl);

  if(PIDThrottleOutput == MIN_ACC_OUT){
    //apply brakes
