Piso 2, falta alguns codigos e regulagem
Página 1 de 1
Piso 2, falta alguns codigos e regulagem
int andarFrente = 50;
int andarEsq = 75;// esquerda e mais rapido do que o direita
int andarDir = 80;
int esq;
int dir;
int result;
float time90graus = 0.7;
float time6cm = 2.1;// 2.2 e um pouco mais do que devia
float time14cm = 4.6;// 5.2 mt
float timePiso2 = 5;
int ultrassomread;
int medida_dist = 0;
int x = 0;
void setup() {
// put your setup code here, to run once:
pinMode(A0, INPUT);//esquerda ++
pinMode(A4, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(53, OUTPUT);
pinMode(A15, INPUT);
pinMode(31, INPUT);
Serial.begin(9600);
delay(5000);
}
void loop() {
//ultrassom();//ve se tem obstaculo, se tiver, desviar
//debug();//imprime os valores dos sensores de linha
mercurio();//ve se o robo esta inclinado, se tiver, duplicar potencia
esq = analogRead(A0) + 1.5 * analogRead(A3); //soma os valores dos sensores da esq
// esq = esq * (-1);//multiplica esse valor por -1
dir = analogRead(A2) + 1.5 * analogRead(A4); //soma os valores dos sensores da dir
result = esq * -1 + dir; //soma os valores:sensores da direita*1 e sensores da esquerda* -1
if (result <= -100) {//ve se a soma dos valores dos sensores da direita mais a soma dos valores da esquerda sao menor q -100
andar_esq(andarEsq, andarEsq);//nesse caso, virar pra esquerda
}
else {
if (result >= 100) {//e se ////se a soma nao for menor q -100
andar_dir(andarDir, andarDir);//nesse caso, virar direita
}
else {//se ñ tiver q virar nem pra direita nem pra esquerda
andar_frente(andarFrente, andarFrente);//andar para frente
//delay(200);
verificaDistancia();
}
}
if (verificarRampa() == true) {
pegarBolinha();
colocarPreto();
}
}
void verificaDistancia() {
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
medida_dist = pulseIn(A15, HIGH);
if (medida_dist <= 800 && medida_dist > 10) {
desviaObstaculo();
}
}
void andar_frente(int pot_d, int pot_e) {//função para andar para frente
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, HIGH);
digitalWrite(4, LOW);
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
}
void andar_tras(int pot_d, int pot_e) {//função para andar para tras
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, LOW);
digitalWrite(4, HIGH);
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
}
void andar_esq(int pot_d, int pot_e) {//função para andar para esquerda
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, HIGH);
digitalWrite(4, HIGH);
digitalWrite(7, LOW);
digitalWrite(6, LOW);
}
void andar_dir(int pot_d, int pot_e) {//função para andar para direita
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
digitalWrite(7, HIGH);
digitalWrite(6, HIGH);
}
void parar() {//função para andar para parar
analogWrite(2, 0);
analogWrite(3, 0);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
digitalWrite(7, LOW);
digitalWrite(6, LOW);
}
void desviaObstaculo() {//função para verificar se o obstaculo esta na frente, se tiver, desviar
parar();
delay(200);
andar_dir(75, 65);
delay(time90graus * 1000);
parar();
delay(1000);
andar_frente(60, 50);
delay(time6cm * 1070);
parar();
delay(1000);
andar_esq(75, 65);
delay(time90graus * 1160);// * 1000 e pouco
parar();
delay(1000);
andar_frente(60, 50);
delay(time14cm * 995);
parar();
delay(1000);
andar_esq(75, 65);
delay(time90graus * 1080);
parar();
delay(1000);
andar_frente(60, 50);
delay(time6cm * 960);
parar();
delay(1000);
andar_dir(75, 65);
delay(time90graus * 1080);
parar();
delay(1000);
}
void debug() {//função q imprime os valores dos sensores de linha
Serial.print("Esquerda ++ = ");
Serial.print(analogRead(A0));
Serial.print(" esquerda + = ");
Serial.print(analogRead(A3));
Serial.print(" Direita ++ = ");
Serial.print(analogRead(A2));
Serial.print(" direita + = ");
Serial.print(analogRead(A4));
Serial.print(" Resultado ");
Serial.println(result);
}
void mercurio() { //função que verifica se esta inclinado, se estiver, dobrar a potencia dos motores
if (digitalRead(31) == 1) {
andarDir = andarDir * 2;
andarEsq = andarEsq * 2;
andarFrente = andarFrente * 2;
delay(10);
andarDir = andarDir / 2;
andarEsq = andarEsq / 2;
andarFrente = andarFrente / 2;
} else {
andarDir = 80;
andarEsq = 80;
andarFrente = 50;
}
}
boolean verificarRampa () {
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int ultrassomEsq = pulseIn(A15, HIGH);
int tempo;
for (tempo = 0; ultrassomEsq <= 800 || tempo <= 4; tempo ++) {
delay(1000);
}
if (tempo == 4) {
return (true);
} else {
return (false);
}
}
void pegarBolinha () {
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int ultrassomEsq = pulseIn(A15, HIGH);
while (ultrassomEsq <= 800) {
// espera sair da rampa
}
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int medidaPiso2 = pulseIn(A15, HIGH);
int rangeMenor = medidaPiso2 - 65;
int rangeMaior = medidaPiso2 + 65;
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
ultrassomEsq = pulseIn(A15, HIGH);
while (ultrassomEsq >= rangeMenor && ultrassomEsq <= rangeMaior) { // andar enquanto nao acha a bolinha
andar_frente(40, 40);
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
ultrassomEsq = pulseIn(A15, HIGH);
}
andar_esq(andarEsq, andarEsq);
delay(time90graus * 1000);
andar_frente(40, 40);
delay(timePiso2 * 1000);
// comando da futura garra
andar_tras(40, 40);
delay(timePiso2 * 1000);
andar_dir(andarDir, andarDir);
delay(time90graus * 1000);
}
void colocarPreto () {
label1: colocarPreto;
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int medidaUltrassom = pulseIn(A15, HIGH);
while (medidaUltrassom >= 600/*|| sensor luz == preto*/) {
andar_frente(40, 40);
}
if (medidaUltrassom <= 600) {
andar_esq(andarEsq, andarEsq);
goto label1;
}
if (true == true/*colocado somente para compilar, o certo seria : sensor luz == preto*/) {
andar_dir(andarDir, andarDir);
delay(time90graus * 2 * 1000);
andar_tras(40, 40);
delay(time6cm / 2 * 1000);
andar_frente(40, 40);
delay(time6cm / 2 * 1000);
}
}
int andarEsq = 75;// esquerda e mais rapido do que o direita
int andarDir = 80;
int esq;
int dir;
int result;
float time90graus = 0.7;
float time6cm = 2.1;// 2.2 e um pouco mais do que devia
float time14cm = 4.6;// 5.2 mt
float timePiso2 = 5;
int ultrassomread;
int medida_dist = 0;
int x = 0;
void setup() {
// put your setup code here, to run once:
pinMode(A0, INPUT);//esquerda ++
pinMode(A4, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(53, OUTPUT);
pinMode(A15, INPUT);
pinMode(31, INPUT);
Serial.begin(9600);
delay(5000);
}
void loop() {
//ultrassom();//ve se tem obstaculo, se tiver, desviar
//debug();//imprime os valores dos sensores de linha
mercurio();//ve se o robo esta inclinado, se tiver, duplicar potencia
esq = analogRead(A0) + 1.5 * analogRead(A3); //soma os valores dos sensores da esq
// esq = esq * (-1);//multiplica esse valor por -1
dir = analogRead(A2) + 1.5 * analogRead(A4); //soma os valores dos sensores da dir
result = esq * -1 + dir; //soma os valores:sensores da direita*1 e sensores da esquerda* -1
if (result <= -100) {//ve se a soma dos valores dos sensores da direita mais a soma dos valores da esquerda sao menor q -100
andar_esq(andarEsq, andarEsq);//nesse caso, virar pra esquerda
}
else {
if (result >= 100) {//e se ////se a soma nao for menor q -100
andar_dir(andarDir, andarDir);//nesse caso, virar direita
}
else {//se ñ tiver q virar nem pra direita nem pra esquerda
andar_frente(andarFrente, andarFrente);//andar para frente
//delay(200);
verificaDistancia();
}
}
if (verificarRampa() == true) {
pegarBolinha();
colocarPreto();
}
}
void verificaDistancia() {
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
medida_dist = pulseIn(A15, HIGH);
if (medida_dist <= 800 && medida_dist > 10) {
desviaObstaculo();
}
}
void andar_frente(int pot_d, int pot_e) {//função para andar para frente
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, HIGH);
digitalWrite(4, LOW);
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
}
void andar_tras(int pot_d, int pot_e) {//função para andar para tras
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, LOW);
digitalWrite(4, HIGH);
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
}
void andar_esq(int pot_d, int pot_e) {//função para andar para esquerda
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, HIGH);
digitalWrite(4, HIGH);
digitalWrite(7, LOW);
digitalWrite(6, LOW);
}
void andar_dir(int pot_d, int pot_e) {//função para andar para direita
analogWrite(2, pot_e);
analogWrite(3, pot_d);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
digitalWrite(7, HIGH);
digitalWrite(6, HIGH);
}
void parar() {//função para andar para parar
analogWrite(2, 0);
analogWrite(3, 0);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
digitalWrite(7, LOW);
digitalWrite(6, LOW);
}
void desviaObstaculo() {//função para verificar se o obstaculo esta na frente, se tiver, desviar
parar();
delay(200);
andar_dir(75, 65);
delay(time90graus * 1000);
parar();
delay(1000);
andar_frente(60, 50);
delay(time6cm * 1070);
parar();
delay(1000);
andar_esq(75, 65);
delay(time90graus * 1160);// * 1000 e pouco
parar();
delay(1000);
andar_frente(60, 50);
delay(time14cm * 995);
parar();
delay(1000);
andar_esq(75, 65);
delay(time90graus * 1080);
parar();
delay(1000);
andar_frente(60, 50);
delay(time6cm * 960);
parar();
delay(1000);
andar_dir(75, 65);
delay(time90graus * 1080);
parar();
delay(1000);
}
void debug() {//função q imprime os valores dos sensores de linha
Serial.print("Esquerda ++ = ");
Serial.print(analogRead(A0));
Serial.print(" esquerda + = ");
Serial.print(analogRead(A3));
Serial.print(" Direita ++ = ");
Serial.print(analogRead(A2));
Serial.print(" direita + = ");
Serial.print(analogRead(A4));
Serial.print(" Resultado ");
Serial.println(result);
}
void mercurio() { //função que verifica se esta inclinado, se estiver, dobrar a potencia dos motores
if (digitalRead(31) == 1) {
andarDir = andarDir * 2;
andarEsq = andarEsq * 2;
andarFrente = andarFrente * 2;
delay(10);
andarDir = andarDir / 2;
andarEsq = andarEsq / 2;
andarFrente = andarFrente / 2;
} else {
andarDir = 80;
andarEsq = 80;
andarFrente = 50;
}
}
boolean verificarRampa () {
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int ultrassomEsq = pulseIn(A15, HIGH);
int tempo;
for (tempo = 0; ultrassomEsq <= 800 || tempo <= 4; tempo ++) {
delay(1000);
}
if (tempo == 4) {
return (true);
} else {
return (false);
}
}
void pegarBolinha () {
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int ultrassomEsq = pulseIn(A15, HIGH);
while (ultrassomEsq <= 800) {
// espera sair da rampa
}
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int medidaPiso2 = pulseIn(A15, HIGH);
int rangeMenor = medidaPiso2 - 65;
int rangeMaior = medidaPiso2 + 65;
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
ultrassomEsq = pulseIn(A15, HIGH);
while (ultrassomEsq >= rangeMenor && ultrassomEsq <= rangeMaior) { // andar enquanto nao acha a bolinha
andar_frente(40, 40);
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
ultrassomEsq = pulseIn(A15, HIGH);
}
andar_esq(andarEsq, andarEsq);
delay(time90graus * 1000);
andar_frente(40, 40);
delay(timePiso2 * 1000);
// comando da futura garra
andar_tras(40, 40);
delay(timePiso2 * 1000);
andar_dir(andarDir, andarDir);
delay(time90graus * 1000);
}
void colocarPreto () {
label1: colocarPreto;
delay(2);
digitalWrite(53, HIGH);
delay(10);
digitalWrite(53, LOW);
int medidaUltrassom = pulseIn(A15, HIGH);
while (medidaUltrassom >= 600/*|| sensor luz == preto*/) {
andar_frente(40, 40);
}
if (medidaUltrassom <= 600) {
andar_esq(andarEsq, andarEsq);
goto label1;
}
if (true == true/*colocado somente para compilar, o certo seria : sensor luz == preto*/) {
andar_dir(andarDir, andarDir);
delay(time90graus * 2 * 1000);
andar_tras(40, 40);
delay(time6cm / 2 * 1000);
andar_frente(40, 40);
delay(time6cm / 2 * 1000);
}
}
Página 1 de 1
Permissões neste sub-fórum
Não podes responder a tópicos