DC모터 + 4WD 자동차 만들기 (TB6612FNG)

int STBY = 10;            // Standby

// Motor A
int A_PWM = 5;            // Speed Control
int A_IN1 = 8;
int A_IN2 = 9;

void setup() {
  pinMode(STBY, OUTPUT);
  pinMode(A_PWM, OUTPUT);
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
}

void loop() {
  move(1, 128, 0);          // motor A(right wheels), half speed, moving forward

  delay(1000);              // go for 1 second
  stop();                   // stop
  delay(250);               // hold for 250ms until move again

  move(1, 128, 1);          // motor A(right wheels), half speed, moving backward

  delay(1000);
  stop();
  delay(250);
}

void move(int motor, int speed, int direction) {
/*
- Move specific motor at speed and direction
- motor: A -> 1
- speed: 0 is off, and 255 is full speed
= direction: 0 clockwise, 1 counter-clockwise
*/
  digitalWrite(STBY, HIGH);        // Disable Standby

  boolean inPin1 = LOW;            // Defalut(direction=0) - Clockwise
  boolean inPin2 = HIGH;

  if(direction == 1) {             // Count-clockwise
    inPin1 = HIGH;
    inPin2 = LOW;
  }

  if(motor == 1){                  // if motor == 1, right wheel
    digitalWrite(A_IN1, inPin1);
    digitalWrite(A_IN2, inPin2);
    analogWrite(A_PWM, speed);
  }
  else {                           // if motor is not 1, left wheel

  }
}

void stop() {
  digitalWrite(STBY, LOW);         // Enable Standby
}

int A_IN1 = 8;
int A_IN2 = 9;

int A_IN1 = 9;
int A_IN2 = 8;

// front wheel : B1 - A1
// rear  wheel : B2 - A2 

int STBY = 10;            // Standby

// Motor A 1 & 2(Right)
int A_PWM = 5;             // Speed Control
int A_IN1 = 8;
int A_IN2 = 9;

// Motor B 1 & 2(Left)
int B_PWM = 6;             // Speed Control
int B_IN1 = 11;
int B_IN2 = 12;

void setup() {
  pinMode(STBY, OUTPUT);
  pinMode(A_PWM, OUTPUT);
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);
  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
}

void loop() {
  move(1, 128, 0);                  // motor A(right wheels), half speed, moving forward
  move(2, 128, 0);                  // motor B(left wheels), half speed, moving forward

  delay(1000);                      // go for 1 second
  stop();                           // stop
  delay(250);                       // hold for 250ms until move again

  move(1, 128, 1);                  // motor A(right wheels), half speed, moving backward
  move(2, 128, 1);                  // motor B(left wheels), half speed, moving backward

  delay(1000);
  stop();
  delay(250);
}

void move(int motor, int speed, int direction) {   //Move specific motor at speed and direction
//motor: A(Right) -> 1, B(Left) -> 2
//speed: 0 is off, and 255 is full speed
//direction: 0 clockwise, 1 counter-clockwise

  digitalWrite(STBY, HIGH);        // Disable Standby

  boolean inPin1 = LOW;            // Defalut(direction=0) - Clockwise
  boolean inPin2 = HIGH;

  if(direction == 1) {             // Count-clockwise
    inPin1 = HIGH;
    inPin2 = LOW;
  }

  if(motor == 1){                  // if motor == 1, right wheel
    digitalWrite(A_IN1, inPin1);
    digitalWrite(A_IN2, inPin2);
    analogWrite(A_PWM, speed);
  } else {                         // if motor is not 1, left wheel
    digitalWrite(B_IN1, inPin1);
    digitalWrite(B_IN2, inPin2);
    analogWrite(B_PWM, speed);
  }
}

void stop() {
  digitalWrite(STBY, LOW);        // Enable Standby
}

#include <boarddefs.h>
#include <IRremote.h>
#include <IRremoteInt.h>
#include <ir_Lego_PF_BitStreamEncoder.h>

//Initialize IR remote
#include <boarddefs.h>
#include <IRremote.h>
#include <IRremoteInt.h>
#include <ir_Lego_PF_BitStreamEncoder.h>

int RECV_PIN = 13;              // IR Signal pin
IRrecv irrecv(RECV_PIN);
decode_results results;

#define BTN_F 0xFF18E7  // Forward Button
#define BTN_B 0xFF4AB5  // Backward Button
#define BTN_L 0xFF10EF  // Left Button
#define BTN_R 0xFF5AA5  // Right Button
#define BTN_O 0xFF38C7  // OK Button

// Initialize TB6612FNG Motor drive
int STBY = 10;  // STBY pin on TB6612FNG. Must be HIGH to enable motor
int A_PWM = 5;  // Right motor speed control using analogWrite() function. Value between 0 - 255
int A_IN1 = 9;  // Right motor - LOW should go forward
int A_IN2 = 8;  // Right motor - HIGH should go forward
int B_PWM = 6;  // Left motor speed control using analogWrite() function. Value between 0 - 255
int B_IN1 = 11; // Left motor - LOW should go forward
int B_IN2 = 12; // Left motor - HIGH should go forward

int R_MaxSpeed = 255;   //set motor speed to max speed
int L_MaxSpeed = 255;   //set motor speed to max speed
int R_TurnSpeed = 128;  //set motor speed to max speed
int L_TurnSpeed = 128;  //set motor speed to max speed

int LR_Direct = 0;      //for Direction

void setup() {
  pinMode(A_PWM, OUTPUT);
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);
  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
  pinMode(STBY, OUTPUT);
  
  Serial.begin(9600);
  irrecv.enableIRIn();                           // Start the receiver
}

void loop() {
  if (irrecv.decode(&results)) {                 // Read IR remote control
    //Serial.println(results.value, HEX);        // for debug
    irrecv.resume();                             // Receive the next value
  }
  
  switch(results.value) {
    
    case BTN_F:
      go_forward();
      break;
      
    case BTN_B:  
      go_backward();
      break;
      
    case BTN_L:  
      if(LR_Direct==0) {
        go_f_left();
      } else if(LR_Direct==1) {
        go_b_left();
      }     
      break;
      
    case BTN_R:
      if(LR_Direct==0) {
        go_f_right();
      } else if(LR_Direct==1) {
        go_b_right();
      }
      break;
      
    case BTN_O:
      stop();
      break;
  }  
  delay(50);
}

// Move specific motor at speed and direction
// motor: A(Right) -> 0, B(Left) -> 1
// speed: 0 is off, and 255 is full speed
// direction: 0 clockwise, 1 counter-clockwise

void move(int motorLR, int speed, boolean inPin1, boolean inPin2) {
  digitalWrite(STBY, HIGH);        // Disable Standby
  
  if (motorLR == 0) {
    analogWrite(A_PWM, speed);
    digitalWrite(A_IN1, inPin1);
    digitalWrite(A_IN2, inPin2);   
  }
  
  if (motorLR == 1) {
    analogWrite(B_PWM, speed);
    digitalWrite(B_IN1, inPin1);
    digitalWrite(B_IN2, inPin2);
  }
}

void go_forward() {
  Serial.println("F");
  move(0, R_MaxSpeed, 0, 1);  // Right motor, Right Speed, forward(0,1)
  move(1, L_MaxSpeed, 0, 1);  // Left motor, Left Speed, forward(0,1)
  LR_Direct = 0;              // Forward
  delay(100);
}

void go_backward() {
  Serial.println("B");
  move(0, R_MaxSpeed, 1, 0);  // Right motor, Right Speed, backward(1,0)
  move(1, L_MaxSpeed, 1, 0);  // Left motor, Left Speed, backward(1,0)
  LR_Direct = 1;              // Backward
  delay(100);
}

void go_f_left() {
  Serial.println("F_L");
  move(0, R_MaxSpeed, 0, 1);  // Right motor, Right Speed, forward(0,1)
  move(1, L_TurnSpeed, 0, 1); // Left motor, Left Speed, forward(0,1)
  delay(100);
}

void go_b_left() {
  Serial.println("F_R");
  move(0, R_MaxSpeed, 1, 0);  // Right motor, Right Speed, backward(1,0)
  move(1, L_TurnSpeed, 1, 0); // Left motor, Left Speed, backward(1,0)

  delay(100);
}

void go_f_right() {
  Serial.println("F_R");
  move(0, R_TurnSpeed, 0, 1); // Right motor, Right Speed, forward(0,1)
  move(1, L_MaxSpeed, 0, 1);  // Left motor, Left Speed, forward(0,1)
  delay(100);
}

void go_b_right() {
  Serial.println("B_R");
  move(0, R_TurnSpeed, 1, 0); // Right motor, Right Speed, backward(1,0)
  move(1, L_MaxSpeed, 1, 0);  // Left motor, Left Speed, backward(1,0)

  delay(100);
}

void stop() {
  Serial.println("STOP");
  digitalWrite(STBY, LOW);    // Enable Standby
}

//Initialize IR remote
#include <boarddefs.h>
#include <IRremote.h>
#include <IRremoteInt.h>
#include <ir_Lego_PF_BitStreamEncoder.h>

int RECV_PIN = 13;       // IR Signal pin
IRrecv irrecv(RECV_PIN);
decode_results results;

#define BTN_F 0xFF18E7  // Forward Button
#define BTN_B 0xFF4AB5  // Backward Button
#define BTN_L 0xFF10EF  // Left Button
#define BTN_R 0xFF5AA5  // Right Button
#define BTN_O 0xFF38C7  // OK Button

// Initialize TB6612FNG Motor drive
int STBY = 10;  // STBY pin on TB6612FNG. Must be HIGH to enable motor
int A_PWM = 5;  // Right motor speed control using analogWrite() function. Value between 0 - 255
int A_IN1 = 9;  // Right motor - LOW should go forward
int A_IN2 = 8;  // Right motor - HIGH should go forward
int B_PWM = 6;  // Left motor speed control using analogWrite() function. Value between 0 - 255
int B_IN1 = 11; // Left motor - LOW should go forward
int B_IN2 = 12; // Left motor - HIGH should go forward

int R_MaxSpeed = 255;   //set motor speed to max speed
int L_MaxSpeed = 255;   //set motor speed to max speed
int R_TurnSpeed = 128;  //set motor speed to max speed
int L_TurnSpeed = 128;  //set motor speed to max speed

int LR_Direct = 0;      //for Direction

// Initialize Ultrasonic Sensor
#define TRIG 2
#define ECHO 4

void setup() {
  pinMode(A_PWM, OUTPUT);                        // Motor
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);
  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
  pinMode(STBY, OUTPUT);
  
  pinMode(TRIG, OUTPUT);                         // Ultrasonic Sensor
  pinMode(ECHO, INPUT);

  Serial.begin(9600);
  irrecv.enableIRIn();                           // Start the receiver
}

void loop() {
  digitalWrite(TRIG, LOW);                       // Start Ultrasonic sensor
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  long distance = pulseIn(ECHO, HIGH) / 58.2;    // 거리 측정

  if (irrecv.decode(&results)) {                 // Read IR remote control
    //Serial.println(results.value, HEX);        // for debug
    irrecv.resume(); // Receive the next value
  }

  if (distance < 10) {                            // 거리가 10cm 이내이면 정지
    stop();
    delay(500);
  }
  else {                                          // 거리가 10cm 이상이면,
  	switch(results.value) {
    
    case BTN_F:
      go_forward();
      break;
      
    case BTN_B:  
      go_backward();
      break;
      
    case BTN_L:  
      if(LR_Direct==0) {
        go_f_left();
      } else if(LR_Direct==1) {
        go_b_left();
      }     
      break;
      
    case BTN_R:
      if(LR_Direct==0) {
        go_f_right();
      } else if(LR_Direct==1) {
        go_b_right();
      }
      break;
      
    case BTN_O:
      stop();
      break;
  }  
  delay(50);
}

// Move specific motor at speed and direction
// motor: A(Right) -> 0, B(Left) -> 1
// speed: 0 is off, and 255 is full speed
// direction: 0 clockwise, 1 counter-clockwise

void move(int motorLR, int speed, boolean inPin1, boolean inPin2) {
  digitalWrite(STBY, HIGH);        // Disable Standby

  if (motorLR == 0) {
    analogWrite(A_PWM, speed);
    digitalWrite(A_IN1, inPin1);
    digitalWrite(A_IN2, inPin2);   
  }

  if (motorLR == 1) {
    analogWrite(B_PWM, speed);
    digitalWrite(B_IN1, inPin1);
    digitalWrite(B_IN2, inPin2);
  }
}

void go_forward() {
  Serial.println("F");
  move(0, R_MaxSpeed, 0, 1);  // Right motor, Right Speed, forward(0,1)
  move(1, L_MaxSpeed, 0, 1);  // Left motor, Left Speed, forward(0,1)
  LR_Direct = 0;              // Forward
  delay(100);
}

void go_backward() {
  Serial.println("B");
  move(0, R_MaxSpeed, 1, 0);  // Right motor, Right Speed, backward(1,0)
  move(1, L_MaxSpeed, 1, 0);  // Left motor, Left Speed, backward(1,0)

  LR_Direct = 1;              // Backward
  delay(100);
}

void go_f_left() {
  Serial.println("F_L");
  move(0, R_MaxSpeed, 0, 1);  // Right motor, Right Speed, forward(0,1)
  move(1, L_TurnSpeed, 0, 1); // Left motor, Left Speed, forward(0,1)
  delay(100);
}

void go_b_left() {
  Serial.println("F_R");
  move(0, R_MaxSpeed, 1, 0);  // Right motor, Right Speed, backward(1,0)
  move(1, L_TurnSpeed, 1, 0); // Left motor, Left Speed, backward(1,0)
  delay(100);
}

void go_f_right() {
  Serial.println("F_R");
  move(0, R_TurnSpeed, 0, 1); // Right motor, Right Speed, forward(0,1)
  move(1, L_MaxSpeed, 0, 1);  // Left motor, Left Speed, forward(0,1)
  delay(100);
}

void go_b_right() {
  Serial.println("B_R");
  move(0, R_TurnSpeed, 1, 0); // Right motor, Right Speed, backward(1,0)
  move(1, L_MaxSpeed, 1, 0);  // Left motor, Left Speed, backward(1,0)
  delay(100);
}

void stop() {
  Serial.println("STOP");
  digitalWrite(STBY, LOW);    // Enable Standby
}