|
Ребята. Мучаюсь уже месяц. Помогите
Взял за основу гуляющий по инету код ПИД алгоритма и вот не могу добиться точного положения. Либо вообще не позиционируется. Смыкает во все стороны ни как не может попсать в точку. Есть энкодер. По прерыванию снимаем импульсы. Но вот добиться точного положения не могу. Перерыл кучу литературы по ПИД - ужас. Может есть решение проще?
Вот куски кода.
// spinMotor(),
int pinMotor1 = 8;
int pinMotor2 = 9;
int pinMotorSpeed = 10;
// pid ()
float kP = 2.25;
float kI = .5;
float kD = 1;
int iMax = 100;
int iMin = 0;
int error = 0;
int lastError = 0;
int sumError = 0;
int minSpeed = 100;
int maxSpeed = 255;
// loop()
int potVal = 0; // значение потенциометра
int inTimeStep = 0;
// encoder()
int interruptPin = 2;
int buttonDir = 3;
volatile int steping = 0;
void setup() {
Serial.begin(9600);
// motor
pinMode(pinMotor1, OUTPUT);
pinMode(pinMotor2, OUTPUT);
pinMode(pinMotorSpeed, OUTPUT);
// encoder
pinMode(interruptPin, INPUT);
pinMode(buttonDir, INPUT);
digitalWrite(interruptPin, HIGH);
digitalWrite(buttonDir, HIGH);
attachInterrupt(0, encoder, RISING);
}
void loop() {
if (Serial.available()>0) potVal = readPotSerial();
inTimeStep = pid(potVal);
spinMotor (inTimeStep);
}
// speedR - скорость вращения от 0 до 255 (положительное значение вращаем в одну сторону, отрицательное - в другую)
void spinMotor (int speedR) {
if (speedR > 0)
{
digitalWrite(pinMotor1,HIGH);
digitalWrite(pinMotor2,LOW);
}
else if (speedR < 0)
{
digitalWrite(pinMotor1,LOW);
digitalWrite(pinMotor2,HIGH);
speedR = speedR * -1;
}
int motorSpeed = map(speedR, 0, 1024, minSpeed, maxSpeed);
if (steping == potVal ) {
digitalWrite(pinMotor1,LOW);
digitalWrite(pinMotor2,LOW);
}
analogWrite(pinMotorSpeed, motorSpeed);
}
// основной алгоритм подсчета
// расчет скорости и направления движения двигателя (ПИД алгоритм)
int pid(int target) {
int error = steping - target;
int correct = kP * error + kD * (error - lastError) + kI * (sumError);
lastError = error;
sumError += error;
if(sumError > iMax){
sumError = iMax;
}
else if(sumError < iMin){
sumError = iMin;
}
return correct;
}
// читает значение с энкодера через прерывание и инкрементирует или декрементирует значение steping
void encoder() {
if (digitalRead(interruptPin) == digitalRead(buttonDir)) {
steping ++;
}
else {
steping --;
}
}
int minSpeed = 100; взята потому, что на меньших значениях появляется писк и не хватает усилия провернуть мотор
Поделитесь идеями, что не так?
|
Вход
Регистрация
FAQ
Поиск
.