%%

a = arduino();

PWM_FREQ =  25000;
SERIAL = True;
DEBUG = False;

% On the Due, pins 6,7,8,9 are hardware PWM pins.

Sensor = 'A0';
Coil = 'A7';
enable = 'A6';
sensorValue = 0;
i=0;
j=0;

configurePin(a,Sensor);
configurePin(a,Coil,'PWM');
 
%   // Ensure 12 bit Read / Write
%   analogReadResolution(12);
%   analogWriteResolution(12);
 
  %Setup PWM output
  configurePin(a,enable,'OUTPUT');
  digitalWrite(a,enablePin, HIGH);
  configurePin(a,Coil,'PWM');
  analogWrite(Coil, 2048); % 50% duty Cycle
  %PWMC_ConfigureClocks(PWM_FREQ * PWM_MAX_DUTY_CYCLE , 0, VARIANT_MCK);





while(true)
    
  sensorValue = analogRead(sensorPin);
  errorV(3) = sensorValue - errorV(1);
  errorV(2) = errorV(2)+sensorValue;
  error(1) = sensorValue;
  analogWrite(a,pwmPin, calcPID(errorV,kp,ki,kd));
  [effort, currState] = calcLQR(measVect,currState,sys);
  %analogWrite(pwmPin, effort);
  delay( 1 ); % 0.5ms
  
end