Friday, August 26, 2011

Motor Drivers: 1A Dual TB6612FNG Part 2

Since the last motor driver post I've cleaned up a lot of the wiring and platform mess.


Instead of having to wire my battery pack supply pins into Vin, I can plug them directly into the Arduino's power adapter. I soldered a barrel jack onto the 9-volt battery adapter and covered the connection in heat-shrink tubing to prevent any mishaps. Not only is it easier to connect to the Arduino, but it keeps everything a lot more clean.

8 x 1.2V Rayovac 4.0 NiMH Batteries
I also upgraded the shipping box to a larger model. More space for the batteries and electronics. This will serve for testing purposes until I get to design and cut out an actual platform in my robotics class.

This is what happens when you order more stuff, bigger box for projects.
Plastic geared motors with matching sumo wheels

Since the robot will be driven by two wheels, I'll need a caster to help keep it balanced. Replaced the guitar pick with an actual metal caster ball.

http://www.sparkfun.com/products/8909
You can sort of see the motor driver in there. I need to get another spool of wire to make longer jumpers
As far as the code goes, I've added LCD feedback that will tell you what the motors are doing (direction and speed).

No the arduino isn't going to hang out of the box, it's just for show!

Enough talk, let's see it in action:

 
The digital camera can record sound, unlike my cellphone.

 
Made a quick change to the code and didn't want to re-record the first video!

Here is the code from the demo video:
Once again, this code is made specifically for the 1A Dual TB6612FNG motor driver. 

//  Noah Stahl
//  8/25/2011
//  http://arduinomega.blogspot.com
//  Arduino Mega 2560
//This sketch is used to control the TB6612FNG Motor Driver.
//The Motor Driver is controlling two bidirectional DC Motors, A and B.
//There are four states for each motor in this code: STANDBY, BRAKE, CW, CCW.
//The speed of the motors can be controlled through PWM. 0 for stopped and
//255 for full-speed.

//!!!READ-ME!!!
//Depending on the placement and wiring of your motors, this code may not work
//as intended on your project. YMMV! You can fix any of the wrong directions by
//changing the FORWARD, REVERSE, RIGHT, and LEFT define values below.
//You will also have to do a little experimenting to determine a good delay
//value for turning. This value is going to change depending on your motor speed,
//torque, surface material, battery voltage, and a whole bunch of other variables
//that you will probably be unable to account for. Just use a good guess.
// LCD Vss(1) to GND
// LCD Vdd(2) to +5V
// LCD Vo (3) to +5V (contrast control)
// LCD RS (4) to digital pin 43
// LCD R/W(5) to GND
// LCD EN (6) to digital pin 42
// LCD D4 (7) to digital pin 40
// LCD D5 (8) to digital pin 39
// LCD D6 (9) to digital pin 38
// LCD D7 (10)to digital pin 37

#include <LiquidCrystal.h>
// Specify the numbers of the LCD pins on the Arduino
LiquidCrystal lcd(43, 42, 40, 39, 38, 37); // RS, EN, D4, D5, D6, D7

#define PWMA 2
#define AIN1 53
#define AIN2 52
#define BIN1 51
#define BIN2 50
#define PWMB 3
#define STBY 49
#define motor_A 0
#define motor_B 1
#define FORWARD 1
#define REVERSE 0
#define RIGHT 1
#define LEFT 0

void setup()
{
  pinMode(PWMA,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(PWMB,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(STBY,OUTPUT);
 
  lcd.begin(20, 2);         // Number of columns and rows on the LCD
  lcd.println("ArduinoMega Blagspot");
  lcd.setCursor(0,1);
  lcd.println("    Motor Driver    ");
  delay(5000);
  lcd.clear();
  motor_standby(false);        //Must set STBY pin to HIGH in order to move
}

void loop()
{
  motor_brake();               //STOP for 1s
  delay(1000);
 
  motor_drive(FORWARD, 255);   //Move FORWARD at full speed for 1s
  delay(1000);
 
  motor_brake();               //STOP for 1s
  delay(1000);

  motor_drive(REVERSE, 150);   //REVERSE at about half-speed for 1s
  delay(1000);
 
  motor_brake();               //STOP for 1s
  delay(1000);
 
  motor_turn(RIGHT, 255, 255); //Spin RIGHT for 0.5s
  delay(500);
}

//Turns off the outputs of the Motor Driver when true
void motor_standby(char standby)
{
  if (standby == true) digitalWrite(STBY,LOW);
  else digitalWrite(STBY,HIGH);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("MOTOR ON");
}

//Stops the motors from spinning and locks the wheels
void motor_brake()
{
  digitalWrite(AIN1,1);
  digitalWrite(AIN2,1);
  digitalWrite(PWMA,LOW);
  digitalWrite(BIN1,1);
  digitalWrite(BIN2,1);
  digitalWrite(PWMB,LOW);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("STOP");
}

//Control the direction the motors turn, speed from 0(off) to 255(full speed)
void motor_drive(char direction, unsigned char speed)
{
  if (direction == FORWARD)
  {
    motor_control(motor_A, FORWARD, speed);
    motor_control(motor_B, FORWARD, speed);
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("FORWARD");
    lcd.setCursor(0,1);
    lcd.print("Speed: ");
    lcd.print((int)speed);
  }
  else
  {
    motor_control(motor_A, REVERSE, speed);
    motor_control(motor_B, REVERSE, speed);
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("REVERSE");
    lcd.setCursor(0,1);
    lcd.print("Speed: ");
    lcd.print((int)speed);
  }
}

//You can control the turn radius by specifying the speed of each motor
//Set both to 255 for it to spin in place
void motor_turn(char direction, unsigned char speed_A, unsigned char speed_B )
{
  if (direction == RIGHT)
  {
    motor_control(motor_A, REVERSE, speed_A);
    motor_control(motor_B, FORWARD, speed_B);
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("RIGHT");
    lcd.setCursor(0,1);
    lcd.print("Speed: ");
    lcd.print((int)speed_A);
  }
  else
  {
    motor_control(motor_A, FORWARD, speed_A);
    motor_control(motor_B, REVERSE, speed_B);
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("LEFT");
    lcd.setCursor(0,1);
    lcd.print("Speed: ");
    lcd.print((int)speed_A);
  }
}

void motor_control(char motor, char direction, unsigned char speed)
{
  if (motor == motor_A)
  {
    if (direction == FORWARD)
    {
      digitalWrite(AIN1,HIGH);
      digitalWrite(AIN2,LOW);
    }
    else
    {
      digitalWrite(AIN1,LOW);
      digitalWrite(AIN2,HIGH);
    }
    analogWrite(PWMA,speed);
  }
  else
  {
    if (direction == FORWARD)  //Notice how the direction is reversed for motor_B
    {                          //This is because they are placed on opposite sides
      digitalWrite(BIN1,LOW);  //To go FORWARD, motor_A spins CW and motor_B spins CCW
      digitalWrite(BIN2,HIGH);
    }
    else
    {
      digitalWrite(BIN1,HIGH);
      digitalWrite(BIN2,LOW);
    }
    analogWrite(PWMB,speed);
  }
}


When it comes time to build my robot, I am most likely going to need to upgrade my plastic motors to metal gearmotors. I'd hate to have the plastic gears fail in the middle of a demonstration. As long as I can keep the current for each motor under 1.2A continuous and 3A peak, I will be able to still use this motor driver and the code.