Prototype

Wiring

Schematic

Breadboard view

Code for the controller

#include

double error = 0;
double dtError = 0;
double old_error = 0;
double potValue = 0;
double oldPotValue = 0;

int ctrlSign =0;
int directrion =0;
int wait = 5;

String temp;

int setValue = 0;
int kp = 12;
int kd = 1;

void setup()
{
  Serial.begin(9600);
  myservo.attach(9);
  myservo.write(105,255,true);
  delay(1000);
  potValue = analogRead(1);
  setValue = potValue - 50;
}

void loop()
{  
  potValue = analogRead(1);
  sendPotValue(potValue);
 
  delay(wait);
  //printInfo();
 
  // Filter input  
  potValue = (0.5 * potValue + 0.5 * oldPotValue) + 0.5;
 
  // Control part
  error = (setValue - potValue);
 
  // Derivitive (delta e / delta t)
  dtError = (error - old_error);///(wait*0.001);
 
  // PD-control
  ctrlSign =  kp * error + kd * dtError;
 
  // Set servo speed and direction
  if(failSafe(error))
  {
    setSpeed(ctrlSign);  
  }
 
  // Update old error
  old_error = error;
  oldPotValue = potValue;
}

void setSpeed(int out)
{
  if(out <= -1)
  {
    myservo.write(170, (int)abs(out), false);
  }
  else if(out >= 1)
  {
    myservo.write(10, (int)abs(out), false);
  }
  else// Do nothing
  {
    myservo.write(myservo.read(), 0, false);
  }
}

int failSafe(int val)
{
  // Error is too big
  if(val < -10 || val > 10)
  {
    myservo.write(105,40,true);
    return 0;
  }
  else
  {
    return 1;
  }
}

void printInfo(){
  Serial.print(setValue);
  Serial.print(", ");
  Serial.print(potValue);
  Serial.print(", ");
  Serial.println(error);
}

int getAngle(int potValue){
  return (int)((setValue - potValue) * 3.5);
}

void sendPotValue(double potValue)
{
  Serial.write('@');// Start stream
  Serial.flush();
 
  writeStream(potValue);
 
  Serial.write('#');// End stream
  Serial.flush();
}

void writeStream(double potValue)
{
  temp = String(potValue);
  byte charBuf[temp.length()];
 
  temp.getBytes(charBuf, temp.length() + 1);
 
  Serial.write(charBuf, temp.length());// Write data
  Serial.flush();
}