sexta-feira, 9 de outubro de 2015

Controlando dois motores com TB6612FNG e Arduino (Ponte H)


Introdução

Todo mês de setembro no Senai é realizado o evento Mundo Senai. A cada versão a comunidade escolar desenvolve projetos vinculados às disciplinas ministradas, esse ano não foi diferente e vários projetos foram criados. Ano passado foi desenvolvido a plataforma Simulate com Raspberry, este ano desenvolvemos outros projetos, mas desta vez com Arduíno.

No início do projeto queríamos criar um robô que segue trilha igual do vídeo abaixo, mas........





mas pensamos, "Cara, robô segue trilha todo mundo já fez, vamos criar algo mais legal hardcore, que ao invés de seguir ou desviar, derrube obstáculos"


Bom, vamos parar de lenga lenga e ir direto ao que interessa.

Oque precisamos 
  1. Arduíno Uno
  2. Driver  TB6612FNG
  3. Sensor de distância
  4. Jumpers
  5. Bateria de 9 volts

Ligação dos fios,sensores, motores e drivers. 

Segue o link do 4shared com o esquema de ligação no Fritzing e o código fonte, no código fonte no fim do post está explicado também as ligações entre o Arduíno e o driver, parece ser difícil, mas é algo bem tranquilo. Acabei não achando o sensor de distância URM37 V3.2 para inserir no Fritzing, mas escrevi o esquema de ligação dos fios no arquivo e também no código fonte.




Fotos e vídeos do projeto















 int URPWM = 3;                   // PWM Output 0-25000us,every 50us represent 1cm  
 int URTRIG = 5;                   // PWM trigger pin  
 int indiceTempoDistancia = 18;  
 int distancia = 0;  
 int Frente = 1;  
 int Tras  = 0;  
 int MotorEsq = 1;  
 int MotorDir = 2;  
 unsigned int Distance = 0;  
 uint8_t EnPwmCmd[4] = {0x44, 0x22, 0xbb, 0x01}; 

 //motor A conecte na porta AO1 e AO2 do driver
//motor B conecte na porta BO1 e BO2 do driver

//Conecte a bateria, polo positivo na porta VMOT do driver,
//e o polo negativo no GND próximo ao VMOT

//Conecte os 5 volts do Arduíno na porta VCC do driver
// e o GND do Arduíno conecte na porta GND próximo ao VCC

 int STBY = 10; //ligue na standby do driver 
 //Motor A  
 int PWMA = 6; //ligue na porta PWMA do driver // Speed control  
 int AIN1 = 9; //ligue na porta AIN1 do driver // Direction 
 int AIN2 = 8; //ligue na porta AIN2 do driver Direction 
 //Motor B  
 int PWMB = 11; //ligue na porta PWMB do driver // Speed control 
 int BIN1 = 12; //ligue na porta BIN1 do driver Direction 
 int BIN2 = 2; //ligue na porta BIN2 do driver Direction

void setup() {  
  Serial.begin(9600);  
  PWM_Mode_Setup();  
  pinMode(STBY, OUTPUT);  
  pinMode(PWMA, OUTPUT);  
  pinMode(AIN1, OUTPUT);  
  pinMode(AIN2, OUTPUT);  
  pinMode(PWMB, OUTPUT);  
  pinMode(BIN1, OUTPUT);  
  pinMode(BIN2, OUTPUT);  
  pinMode(A5 ,OUTPUT);  
  move(MotorEsq, 255, Frente);  
  move(MotorDir, 255, Tras);  
  delay(3000);  
  stop();  
 }  
 void derrubar() {  
  delay(1000);  
  move(MotorEsq, 255, Tras);  
  move(MotorDir, 255, Tras);  
  delay(distancia*indiceTempoDistancia);  
  stop();  
 }  
 void voltar() {  
  delay(1000);  
  move(MotorEsq, 255, Frente);  
  move(MotorDir, 255, Tras);  
  delay(150);  
  stop();  
 }  
 void loop() {  
  for (int i = 0; i < 20; i++) {  
   move(MotorEsq, 50, Frente); //motor 1, full speed, left  
   move(MotorDir, 50, Tras); //motor 2, full speed, left  
   delay(100);  
   stop();  
   delay(100);  
   distancia = PWM_Mode();  
   Serial.println(distancia);  
   if (distancia < 40){  
     delay(250);  
     digitalWrite(A5,LOW);  
     delay(250);  
     digitalWrite(A5,HIGH);  
     delay(250);  
     digitalWrite(A5,LOW);  
     delay(250);  
     digitalWrite(A5,HIGH);  
     delay(250);  
     digitalWrite(A5,LOW);  
     delay(250);  
     derrubar();  
     voltar();  
   }   
  }  
 }  
 void move(int motor, int speed, int direction) {  
  //Move specific motor at speed and direction  
  //motor: 0 for B 1 for A  
  //speed: 0 is off, and 255 is full speed  
  //direction: 0 clockwise, 1 counter-clockwise  
  digitalWrite(STBY, HIGH); //disable standby  
  boolean inPin1 = LOW;  
  boolean inPin2 = HIGH;  
  if (direction == 1) {  
   inPin1 = HIGH;  
   inPin2 = LOW;  
  }  
  if (motor == 1) {  
   digitalWrite(AIN1, inPin1);  
   digitalWrite(AIN2, inPin2);  
   analogWrite(PWMA, speed);  
  } else if (motor == 2) {  
   digitalWrite(BIN1, inPin1);  
   digitalWrite(BIN2, inPin2);  
   analogWrite(PWMB, speed);  
  }  
 }  
 void stop() {  
  //enable standby  
  digitalWrite(STBY, LOW);  
 }  
 void PWM_Mode_Setup() {  
  pinMode(URTRIG, OUTPUT);              // A low pull on pin COMP/TRIG  
  digitalWrite(URTRIG, HIGH);            // Set to HIGH  
  pinMode(URPWM, INPUT);               // Sending Enable PWM mode command  
  for (int i = 0; i < 4; i++) {  
   Serial.write(EnPwmCmd[i]);  
  }  
 }  
 int PWM_Mode() {                  // a low pull on pin COMP/TRIG triggering a sensor reading  
  digitalWrite(URTRIG, LOW);  
  digitalWrite(URTRIG, HIGH);           // reading Pin PWM will output pulses  
  unsigned long DistanceMeasured = pulseIn(URPWM, LOW);  
  if (DistanceMeasured == 50000) {         // the reading is invalid.  
   Serial.print("Invalid");  
  }  
  else {  
   Distance = DistanceMeasured / 50;       // every 50us low level stands for 1cm  
  }  
  return Distance;  
 }