Arduino自走車控制

/*
 車車資訊
 輪距 = 13cm
 輪半徑 = 3.3cm
 */
#include <AFMotor.h>
AF_DCMotor motorLeft(2);
AF_DCMotor motorRight(3);

int DigitalPinLeft = A5;   
int DigitalPinRight = A2; 

  
int cnt1 = 0;  
int last1 = 0;
int cnt2 = 0;  
int last2 = 0;
unsigned long timer;
unsigned long dt;

const float tire_radius = 3.3; //輪半徑 = 3.3cm
const int tire_distance = 13; //輪距 = 13cm


void move(int cm){
  int turn = (cm/(2*PI*tire_radius*0.2))*2 + 1;//每個turn約等於36度
  cnt1 = 0;
  cnt2 = 0;
  motorLeft.run(FORWARD);
  motorRight.run(FORWARD);
  while(cnt1 < turn || cnt2 <turn){
  boolean D_val1 = digitalRead(DigitalPinLeft);
  boolean D_val2 = digitalRead(DigitalPinRight);
  if(abs(last1 - D_val1) == 1){
    cnt1++;
    if(cnt1 >= turn){
      motorLeft.run(RELEASE); 
    }
  }
  if(abs(last2 - D_val2) == 1){
    cnt2++;
    if(cnt2 >= turn){
      motorRight.run(RELEASE); 
    }
  }
  last1 = D_val1; 
  last2 = D_val2;
  }
}

void turnLeft(int degree){
  int turn = (degree*tire_distance/tire_radius)/72*2+1;
  cnt1 = 0;
  motorLeft.run(FORWARD);
  while(cnt1 < turn){
  boolean D_val1 = digitalRead(DigitalPinLeft);
  boolean D_val2 = digitalRead(DigitalPinRight);
  if(abs(last1 - D_val1) == 1){
    cnt1++;
    if(cnt1 >= turn){
      motorLeft.run(RELEASE); 
    }
  }
  last1 = D_val1; 
  }
}


void setup()
{
  Serial.begin(115200);
  pinMode(DigitalPinLeft, INPUT);  
  pinMode(DigitalPinRight, INPUT);  
  motorLeft.setSpeed(75);
  motorRight.setSpeed(125);     
  delay(2000);  
}

void loop()
{
  move(50);
  delay(1000000);

}