디지털로 다 바꿨는데도 여전히 안되네요. 어떻게 해야할까요? 아니면 혹시 아날로그로 다 바꿀 수 있는 방법은 없나요? m12랑 m22가 여전히 안되네요.
#include "Wire.h"
#define IR_TRIG 9
#define IR_ECHO 8 
union {int16_t val; byte med[sizeof(val)];} dat; //union 사용.
const byte m11=10; const byte m12=12; const byte m21=11; const byte m22=13;
const byte DT=5; const float pi=3.141592;
int16_t accX, accY, accZ, gyroX, gyroY, gyroZ; 
uint32_t timer; uint8_t i2cData[14]; float  gyroYangle, compAngleY, gyroYrate; 


float A=0.96, croll0=0, croll, roll;
const float scaling=0.0076336; // MPU6050: +/-250degree per 16bit (500/65536)
uint32_t rt0=0, oldt3; int16_t Output33; 
float inc=50.0, bangle=0.0; //inc: angle band in control, bangle: balancing angle to be maintained

float Input3, Output3, Output31, olderror3=0, oldintegral3=0;
float dt=(DT)/1000.0, fc=120.0 ; //filtered data, sampling time, cutoff freq.
float alpha=dt/((1/(2*pi*fc))+dt); //LPF for the gyrosensor
float fc1=4.0; //Cutoff frequency [Hz] for the derivative gain
float alpha1=dt/((1/(2*pi*fc1))+dt); //LPF the derivative gain
int16_t yaw, rotation=150;
const float runangle=18.0; //forward/backward reference angle to be tracked

// Very Important!! Control Gain
float Kpg =90, Kig = 20, Kdg =0.1; //PID gain: Gyroscopic sensor at 12v
//

void Receive() //Receiving the refrence input from the tranmitter via the Bluetooth
{while(Serial2.available()>=((2*sizeof(dat.med))+1)) 
  {if (Serial2.read()=='@'){
  for (int16_t i=0; i<sizeof(dat.med); i++) {dat.med[i]=Serial2.read();} 
  bangle= -(((float) dat.val)/500.0)*runangle; Serial.println(bangle); //-5 degree~ 5 degree
  for (int16_t i=0; i<sizeof(dat.med); i++) {dat.med[i]=Serial2.read();} 
  yaw=map(dat.val, -500, 500, rotation, -rotation); Serial.println(yaw); }
  else {}}}

void motion() //Move the balancing machine upon request from the arduino
{ if (abs(Input3)<inc){
Output3=PID3(bangle-0.0,-Input3);
Output31=constrain(Output3, -255,255);
int16_t Output311=Output31;
int motorA=Output311+yaw, motorB=Output311-yaw; 
if(motorA>0) {analogWrite(m11,min(motorA,200));analogWrite(m12,min(motorA,200));
  if(motorB>0) {analogWrite(m21,min(motorB,200));analogWrite(m22,0);}
  else {analogWrite(m21,min(abs(motorB),200));analogWrite(m22,min(abs(motorB),200));}}
else {analogWrite(m11,min(abs(motorA),200)); analogWrite(m12,0);
  if(motorB>0) {analogWrite(m21,min(motorB,200));analogWrite(m22,0);}
  else {analogWrite(m21,min(abs(motorB),200));analogWrite(m22,min(abs(motorB),200));}}}
else {analogWrite(m11,0);analogWrite(m21,0);}

}


float gyroangle(){
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);  accY = ((i2cData[2] << 8) | i2cData[3]);  accZ = ((i2cData[4] << 8) | i2cData[5]);  
gyroX = (i2cData[8] << 8) | i2cData[9];  gyroY = (i2cData[10] << 8) | i2cData[11];  gyroZ = (i2cData[12] << 8) | i2cData[13];
float dt = (float)(micros() - timer) / 1000000.0;
timer = micros();
float aroll=atan2(accY,accZ)*180.0/pi; //in degree, -180~180
float groll=scaling*(gyroX*dt)+croll0;
croll=A*groll+(1-A)*aroll; //Complementary filter, cf) float K = 0.8; float A = K / (K + dt);
croll0=croll; 
roll=(1-alpha)*roll+alpha*croll; 
float y=(1-alpha)*y+alpha*roll; 
//Serial.println(y);
return y;}

float PID3(float Setpoint, float Input) //PID Control w.r.t. Gyro sensor
{float newerror3=Setpoint-Input; //
uint32_t newt3=millis();
uint32_t dt3=newt3-oldt3; //in milliseconds
float Kpgg,Kigg,Kdgg;
if(newerror3*oldintegral3<0){oldintegral3=0.0; Kpgg=1*Kpg; Kigg=2*Kig; Kdgg=1*Kdg;} //Gain Weighting
else {Kpgg=Kpg; Kigg=Kig; Kdgg=Kdg;}
float newintegral3=oldintegral3+newerror3*dt3/1000;
float Kpgg0=Kpgg*newerror3;
float Kpgg1=constrain(Kpgg0,-350.0,350.0);
float newerror33=(1-alpha1)*newerror33+alpha1*newerror3; //LPF for the D term
float derivative3=((newerror33-olderror3)/dt3)*1000;
float derivative330=Kdgg*derivative3;
float derivative33=constrain(derivative330,-350.0,350.0);
float iterm=Kigg*newintegral3;
float iterm3=constrain(iterm,-330.0,330.0);
float Output3=Kpgg1+iterm3+derivative33;
olderror3=newerror3; oldintegral3=newintegral3; oldt3=newt3;
//Serial2.print(Kpgg1);Serial2.print("\t");Serial2.print(iterm3);Serial2.print("\t");Serial2.println(derivative33);
return Output3;}


void setup() {
pinMode(m12,1); pinMode(m22,1); 
Wire.begin();  while (i2cWrite(0x6B, 0x01, true)); // PLL(phase locked loop)출력주파수 일정하게 유지 with X axis gyroscope reference and disable sleep mode
Serial.begin(9600); Serial2.begin(9600);Serial2.flush();
pinMode(m11, OUTPUT);
pinMode(m21, OUTPUT);
pinMode(m12, OUTPUT);
pinMode(m22, OUTPUT);  
 
  //초음파 센서 핀모드 설정
  pinMode(IR_TRIG, OUTPUT);
  pinMode(IR_ECHO, INPUT);

}

void loop() {
    float duration, distance;
  digitalWrite(IR_TRIG, HIGH);
  delay(10);
  digitalWrite(IR_TRIG, LOW);
 
  // echoPin 이 HIGH를 유지한 시간
  duration = pulseIn(IR_ECHO, HIGH);
  // HIGH 였을 때 시간(초음파가 보냈다가 다시 들어온 시간)을 가지고 거리를 계산 한다.
  // 340은 초당 초음파(소리)의 속도, 10000은 밀리세컨드를 세컨드로, 왕복거리이므로 2로 나눠주면 거리가 cm 로 나옴
  distance = ((float)(340 * duration) / 10000) / 2;
  Serial.print("\nDIstance : ");
  Serial.println(distance);
     
  if(distance < 20) {         //장애물 감지 (20cm 이내)
    back();
    
           int rnd = random(0,2);   //장애물 감지시 좌/우회전 랜덤처리
     Serial.println(rnd);
     if(rnd == 0){
        Serial.println("right");
        right();
     }else{
        Serial.println("left");
        left();
     }
   }

  Receive(); Input3=gyroangle(); motion(); delay(DT);

}

void left(){
  digitalWrite(m11, LOW); //오른쪽 뒤
  digitalWrite(m12,HIGH); 
 digitalWrite(m21,LOW);
 digitalWrite(m22,HIGH);
  delay(3000);
}
void right(){
  digitalWrite(m11, HIGH);
  digitalWrite(m12, LOW);
  digitalWrite(m21, HIGH); //왼쪽 앞
  digitalWrite(m22, LOW);
  delay(3000);
}
void back(){
  digitalWrite(m11, HIGH);
  digitalWrite(m12, LOW);
  digitalWrite(m21, LOW);
  digitalWrite(m22, HIGH);
  delay(3000);
}