arduinoでフィードバック制御

概要

arduino unoでフィードバック制御をやってみた。

写真

動画

回路図

サンプルコード

#include <SPI.h>
#include <L3GD20.h>
#include <PID_v1.h>

unsigned long time;
L3GD20 gyro(10);
float dc_offsetY = 0;
float rateY;
float prev_rateY = 0;
float angleY = 0;
int sampleTime = 10;
double Setpoint = 0;
double Input = 0;
double Output = 0;
int rec[10];
PID robo(&Input, &Output, &Setpoint, 200.0, 50.0, 30.0, REVERSE);
void setup()
{
    int sampleNum = 1024;
    int X, Y, Z;
    float x, y, z;
    Serial.begin(115200);
    pinMode(4, OUTPUT);
    pinMode(7, OUTPUT);
    Setpoint = 250.0;
    robo.SetOutputLimits(-255.0, 255.0);
    robo.SetSampleTime(sampleTime);
    robo.SetMode(AUTOMATIC);
    Serial.print(" start\t");
    gyro.begin();
    Serial.println(" ok!");
    for (int n = 0; n < sampleNum; n++)
    {
        Y = gyro.read(L3GD20_Y_H);
        y = (Y << 8) | gyro.read(L3GD20_Y_L);
        dc_offsetY += y;
    }
    dc_offsetY = dc_offsetY / sampleNum;
    Serial.print(" Y: ");
    Serial.print(dc_offsetY);
    for (int i = 0 ; i < 10 ; i++)
    {
        rec[i] = 0;
    }
}
void loop()
{
    int X, Y, Z;
    float x, y, z;
    int count;
    int i;
    if (millis() - time >= sampleTime)
    {
        time = millis();
        Y = gyro.read(L3GD20_Y_H);
        y = (Y << 8) | gyro.read(L3GD20_Y_L);
        rateY = (y - dc_offsetY) / 100;
        angleY += ((float) (prev_rateY + rateY) * sampleTime) / 2000;
        rec[0] = rateY - prev_rateY;
        prev_rateY = rateY;
        if (angleY < 0)
        {
            angleY += 360;
        }
        else if (angleY >= 360)
        {
            angleY -= 360;
        }
        count = 0;
        for (i = 0; i < 10; i++)
        {
            if (abs(rec[i]) < 1)
            {
                 count++;
            }
        }
        if (count > 9)
        {
            Setpoint = angleY;
            Serial.print(Setpoint);
            Serial.println(" setpoint");
        }
        for (i = 9; i > 0; i--)
        {
            rec[i] = rec[i - 1];
        }
        Input = angleY;
        if (robo.Compute())
        {
        }
        else
        {
            Serial.print(" ng ");
        }
        int speed = Output;
        if (speed > 0)
        {
            digitalWrite(4, HIGH);
            analogWrite(5, 256 - speed);
            analogWrite(6, 256 - speed);
            digitalWrite(7, HIGH);
        }
        else
        {
            digitalWrite(4, LOW);
            analogWrite(5, -speed);
            analogWrite(6, -speed);
            digitalWrite(7, LOW);
        }
    }
}