Demo entry 6342140

Fase 3

   

Submitted by Arnau on Jan 05, 2017 at 18:58
Language: Arduino. Code size: 1.5 kB.

#include <TimerOne.h>

unsigned long lastTime;

double errSum=0, lastErr=0, lastLastErr=0;
double out = 0, lastOut = 0;

double kp = 500, ki = 22, kd = 0.3;
double k1, k2, k3;

int sensorMax = 553, sensorMin = 0, angle = 0;
int aReal, aObjectiu, err;

unsigned long now, deltaT;

const int aMin = -45, aMax = 75;

void setup() {
  Serial.begin(9600);
  Timer1.initialize(2000);
  Timer1.attachInterrupt(interrupcio);
  //Timer1.pwm(9, 1024); // sensorMax
  //sensorMax = analogRead(A0);
  //delay(3000);
  //Timer1.setPwmDuty(9, 0);
  //delay(3000);
  Timer1.pwm(9, 0);
}

void loop() {
  angle = analogRead(A0);
  angle = map(angle, sensorMin, sensorMax, aMin, aMax);
  Serial.println(angle); // Angle real
  delay(2); // Deixem temps per la transmissio de dades
}

void interrupcio(){
  
  now = millis(); //Temps actual
  aReal = analogRead(A0);
  aReal = map(aReal,sensorMin,sensorMax,0,1023);
  aObjectiu = analogRead(A1);
  aObjectiu = map(aObjectiu,0,1023,0,1023);
  
  err = aObjectiu - aReal;
  deltaT = now - lastTime;
  
  k1 = kp + ki * deltaT / 2 + kd / deltaT;
  k2 = - kp + ki * deltaT / 2 - 2 * kd / deltaT;
  k3 = kd / deltaT;
  
  out = lastOut + k1 * err + k2 * lastErr + k3 * lastLastErr; // en teoria 0<out<1023?
  out = constrain(out,0,1023);
  //Fem una conversio per les diferències per la funcio out
  lastOut = out;
  lastLastErr = lastErr;
  lastErr = err;
  lastTime = now;
  Timer1.setPwmDuty(9,out);
  
}

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).