lab09Control.ino

const int MPIN = 2; //motor pwm out pin
const int DPIN = A0; // voltage distance sensor pin

float steady_state_error;
float voltage;
float target;
float accumulate;

void setup()
{
  pinMode(DPIN, INPUT);
  pinMode(MPIN, OUTPUT);
  accumulate = 0;
  target = 1.5; //target voltage
  Serial.begin(9600);
}

void loop()
{
  voltage = analogRead(DPIN) * 0.00488758553; //analogread * (5.0 / 1023.0)
  steady_state_error = -1*max(min((target-voltage), 1), -1);
  accumulate += 0.1 * steady_state_error;
  accumulate = min(max(accumulate, -1), 1);
  Serial.print(voltage);
  Serial.print('\t');
  Serial.print(target);
  Serial.print('\t');
  Serial.print(steady_state_error);
  Serial.print('\t');
  Serial.print(accumulate);
  Serial.print('\n');
  for(int i = 0; i < 20; ++i)
  {
    digitalWrite(MPIN, HIGH);
    delayMicroseconds(accumulate*250 + steady_state_error*250+1500);
    digitalWrite(MPIN, LOW);
    delayMicroseconds(3000-accumulate*250 + steady_state_error*250);
  }
}