Bender/bender.cpp at master · Pupilo/Bender · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/*
Autor: Willian.A.Mayan
Projeto: Biblioteca de movimentos e funções do Bender
Versão: 0.0.1
*/

#include "WProgram.h"
#include "bender.h"

//Variaveis para inversão de polaridade do motor da esquerda
//#define esq_neg = 5 //Esquerda Negativo -> INI2 do relé
//#define esq_pos = 4 //Esquerda Positivo -> INI1 do relé

//Variaveis para inversão de polaridade do motor da direita
//#define dir_neg = 2 //Direita Negativo -> INI4 do relé
//#define dir_pos = 3 //Direita Positivo -> INI3 do relé
//--------------------- Funções ------------------------
Bender::Bender(int e_neg, int e_pos, int d_neg, int d_pos)
{
  //pinMode(pin, OUTPUT);
  //_pin = pin;
  //Pinos do motor da esquerda
  pinMode(e_pos, OUTPUT);
  pinMode(e_neg, OUTPUT);
  //Pinos do motor da direita
  pinMode(d_pos, OUTPUT);
  pinMode(d_neg, OUTPUT);
  //for(i = 0; i <= 3; i++){
  //  digitalWrite(i, HIGH);
  //}
 Besq_neg = e_neg;
 Besq_pos = e_pos;
 Bdir_neg = d_neg;
 Bdir_pos = d_pos;
}

//Função que move o robo para tras
void Bender::sul(){
  //Serial.print("Sul\n");
  digitalWrite(Besq_pos, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_pos, LOW); //Ligar rotação do motor
  delay(2200);                 //Tempo de espera para desligar o motor
  digitalWrite(Besq_pos, HIGH);  //Desligar rotação do motor
  digitalWrite(Bdir_pos, HIGH);  //Desligar rotação do motor
  delay(2200);                 //Tempo de espera para desligar o motor
  digitalWrite(Besq_pos, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_pos, LOW); //Ligar rotação do motor
}

//Função que move o robo para tras
void Bender::norte(){
  //Serial.println("Norte\n");
  digitalWrite(Besq_neg, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_neg, LOW); //Ligar rotação do motor
  delay(2200);                 //Tempo de espera para desligar o motor
  digitalWrite(Besq_neg, HIGH);  //Desligar rotação do motor
  digitalWrite(Bdir_neg, HIGH);  //Desligar rotação do motor
  delay(2200);                 //Tempo de espera para desligar o motor
  digitalWrite(Besq_neg, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_neg, LOW); //Ligar rotação do motor

}

//Função que move o robo para direita
void Bender::direita(){
  //Serial.println("Direita\n");
  digitalWrite(Besq_pos, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_neg, LOW); //Ligar rotação do motor
  delay(1200);                 //Tempo de espera para desligar o motor
  digitalWrite(Besq_pos, HIGH);  //Desligar rotação do motor
  digitalWrite(Bdir_neg, HIGH);  //Desligar rotação do motor
  delay(1200);
  digitalWrite(Besq_pos, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_neg, LOW); //Ligar rotação do motor
}

//Função que move o robo para esquerda
void Bender::esquerda(){
  //Serial.println("Esquerda\n");
  digitalWrite(Besq_neg, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_pos, LOW); //Ligar rotação do motor
  delay(1200);                 //Tempo de espera para desligar o motor
  digitalWrite(Besq_neg, HIGH);  //Desligar rotação do motor
  digitalWrite(Bdir_pos, HIGH);  //Desligar rotação do motor
  delay(1200);
  digitalWrite(Besq_neg, LOW); //Ligar rotação do motor
  digitalWrite(Bdir_pos, LOW); //Ligar rotação do motor
}