non-PID 제어

Tester

STL files



Circuits


Arduino

#include "MPU6050_DMP6.h"
#include "Servo.h"

Servo servo;

void setup() {
  Serial.begin(9600);

  servo.attach(9);

  int mpuInit = mpuSetup();
  if (mpuInit == 0) {
    Serial.println("MPU6050 connected");
  } else {
    Serial.print("DMP Init Fail(");
    Serial.print(mpuInit);
    Serial.println(")");
  }
}

int motorSpeed = 700;

void loop() {
  mpuLoop();

  float roll = mpuRoll();
  if(roll > 0) {
    motorSpeed++;
  } else if(roll < 0) {
    motorSpeed--;
  }

  if(motorSpeed > 2500) motorSpeed = 2500;
  if(motorSpeed < 700) motorSpeed = 700;

  servo.writeMicroseconds(motorSpeed);
  Serial.print("Angle: ");
  Serial.print(roll);
  Serial.print("\t Speed: ");
  Serial.println(motorSpeed);
  delay(50);
}



Result