int count = 0;
bool direction; //clockwise = true
long prevT = 0; //previous time
float eprev = 0; //previous error
float eintegral = 0; //integral value
void setup()
{
DDRD = 0b01100000; //pin 6, 5 as output
PCICR |= B00000100; //enable PCMSK2 (group 2: PCINT16 to PCINT23)
PCMSK2 |= B00000100; //D2 could trigger interrupt
Serial.begin(9600);
}
void loop()
{
//PID constants
float kp = 1;
float kd = 0.025;
float ki = 0.0;
// time difference
long currT = micros();
float deltaT = ((float) (currT - prevT))/( 1.0e6 );
prevT = currT;
// set target 1200 steps
//int target = currT/( 1.0e6 )*100+1200;
int target = sin(2*3.14*currT/( 1.0e6 )/8)*500+1200+100*currT/( 1.0e6 );
// error
int e = count-target;
// derivative
float dedt = (e-eprev)/(deltaT);
// integral
eintegral = eintegral + e*deltaT;
// control signal
float u = kp*e + kd*dedt + ki*eintegral;
// motor power
float pwr = fabs(u);
if( pwr > 255 ){
pwr = 255;
}
if(u<0){
direction = true;
}
else{
direction = false;
}
rotate(direction, pwr);
delay(50);
//Serial.print("current: ");
Serial.print(count);
Serial.print(" ");
Serial.print("target: ");
Serial.print(target);
Serial.print(" ");
Serial.print("PID: ");
Serial.println(u);
}
ISR (PCINT2_vect){
//on rising edge of D2
if(PIND & 0b00000100){
//if D3 is low, counterclockwise rotation
if(PIND & 0b00001000){
count++;
}
else{
count--;
}
}
}
void rotate(bool direction, float speed){
if(direction){
PORTD = PIND & 0b10111111; //set pin 6 low
analogWrite(5, speed); //ouput from pin 5
}
else{
PORTD = PIND & 0b11011111; //set pin 5 low
analogWrite(6, speed); //ouput from pin 6
}
}
reference:
PID control
H-bridge motor driver
pin change interrupt
No comments:
Post a Comment