[Arduino] 코드 수정해보기

오늘은 필요한 코드만 모아서 수정해보았습니다.
Error Handler 작업은 해주지 않았습니다.
솔직히 어떤 에러가 발생할지 예측이 안가는 상태에서 그거까지 하는건 오버이기 때문이죠.

#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;

uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

/*********파라메터 튜닝 시작*********/
double setpoint= 180;
double Kp = 21; 
double Kd = 0.8; 
double Ki = 140; 
/******파라메터 튜닝 끝*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
    // 기본 옵셋값 설정
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688); 

    // 정상동작하는 경우
    if (devStatus == 0)
    {
        // DMP 가동
        mpu.setDMPEnabled(true);

        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // 패킷사이즈 가져오기
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //PID 설정
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }

    //모터 출력핀 초기화
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);

   //모터 동작 OFF
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW);
}

void loop() { 
  if(error) return 0;

    while (fifoCount < packetSize)
    {
        pid.Compute();   
=               
        if (input>150 && input<200){//로봇이 기울어지는 경우(각도 범위내에서만)
          
        if (output>0) //앞으로 기울어지는 경우
        Forward(); //전진
        else if (output<0) //뒤로 기울어지는 경우
        Reverse(); //후진
        }
        else //로봇이 기울어지지 않은 경우
        Stop(); //모터 정지
        
    }
}

void Forward() //전진
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F"); 
}

void Reverse() //후진
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1); 
    Serial.print("R");
}

void Stop() //정지
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0); 
    Serial.print("S");
}
bool PID::Compute()
{
   if(!inAuto) return false;
   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);

   if(timeChange>=SampleTime)
   {
      double input = *myInput;
      double error = *mySetpoint - input;
      double dInput = (input - lastInput);

      outputSum+= (ki * error);

      if(!pOnE) outputSum-= kp * dInput;

      if(outputSum > outMax) outputSum= outMax;
      else if(outputSum < outMin) outputSum= outMin;

	   double output;
      if(pOnE) output = kp * error;
      else output = 0;

      output += outputSum - kd * dInput;

	    if(output > outMax) output = outMax;
      else if(output < outMin) output = outMin;
	    *myOutput = output;

      lastInput = input;
      lastTime = now;
	    return true;
   }
   else return false;
}

헤더파일이 정말 정말 많은데 이런식으로 필요한거만 뽑아내서 새로 작성해보니 별로 걸리진 않습니다.
그러나 세세한 제어나 에러 발생시에 대처할 수 있지는 않아서 이게 맞나 싶습니다..