[segunda-fundacao] código arduino do MNerim

#include <ServoTimer2.h> // Inclusao da biblioteca de controle de
Servo via timer2

ServoTimer2 servo1; // Declaracao dos motores - maximo de 8 para a
Servotimer2
ServoTimer2 servo2;
ServoTimer2 servo3;
ServoTimer2 servo4;
ServoTimer2 servo5;
ServoTimer2 servo6;
ServoTimer2 servo7;

bool F1exec; //declaracos de flags que definem se funcao esta ativa
(comando em execussao)
bool F2exec;
bool F3exec;
bool F4exec;

void setup() {
Serial.begin(115200);
Serial.println("Uai...eu sou o MNERIM");
Serial.println("Estamos Conectados a 115200bps");
servo1.attach(2); // Definicao dos pinos(GPIO) de controlde de cada motor
servo2.attach(3);
servo3.attach(4);
servo4.attach(5);
servo5.attach(6);
servo6.attach(7);
servo7.attach(8);
//todas as fucoes desabilitadas - 0 = falso
F1exec = 0;
F2exec = 0;
F3exec = 0;
F4exec = 0;

ZeroPos(); // ir para posicao inicial
}

// esta função inicializa os motores
//de acordo com a precisão mecânica da montagem eles devem ser ajustados
//posicao minima => 730 posicao maxima =>2270 range => 1540 meio => 1500
int ZeroPos(void){
servo1.write(1500);//1500
servo2.write(1500);//1500
servo3.write(1450);//1000
servo4.write(1500);//2000
servo5.write(1450);//1800
servo6.write(1550);//1200
servo7.write(1500);//1500
return 0;
}

int ExecutaComando(uint8_t dado)
{
uint8_t nimble1;
uint8_t nimble2;
int pos;
int newpos;
nimble1 = dado / 16;
nimble2 = dado % 16;
switch (nimble1)
{
case 1:
pos = servo1.read();
newpos = pos+((nimble2-5)*20);
servo1.write(newpos);
return newpos;
break;
case 2: //giro invertido
pos = servo2.read();
newpos = pos-((nimble2-5)*20);
servo2.write(newpos);
return (3000 - newpos);
break;
case 3: //giro invertido
pos = servo3.read();
newpos = pos-((nimble2-5)*20);
servo3.write(newpos);
return (3000 - newpos);
break;
case 4:
pos = servo4.read();
newpos = pos+((nimble2-5)*20);
servo4.write(newpos);
return newpos;
break;
case 5:
pos = servo5.read();
newpos = pos+((nimble2-5)*20);
servo5.write(newpos);
return newpos;
break;
case 6: //giro invertido
pos = servo6.read();
newpos = pos-((nimble2-5)*20);
servo6.write(newpos);
return (3000 - newpos);
break;
case 7:
pos = servo7.read();
newpos = pos+((nimble2-5)*20);
servo7.write(newpos);
return newpos;
break;
case 9:
Serial.println("9");
break;
case 15:
if (nimble2 == 0)
return ZeroPos();
if (nimble2 == 1){
F1exec = 1;
return 1;
}
if (nimble2 == 1){
F1exec = 1;
return 1;
}
if (nimble2 == 2){
F2exec = 1;
return 2;
}
if (nimble2 == 3){
F3exec = 1;
return 3;
}
if (nimble2 == 4){
F4exec = 1;
return 4;
}
break;
case 14:
if (nimble2 == 0)
return ZeroPos();
if (nimble2 == 0){
F1exec = 0;
return 0;
}
if (nimble2 == 0){
F1exec = 0;
return 0;
}
if (nimble2 == 0){
F2exec = 0;
return 0;
}
if (nimble2 == 0){
F3exec = 0;
return 0;
}
if (nimble2 == 0){
F4exec = 0;
return 0;
}
break;
default:
Serial.println(nimble1,DEC);
break;
}//swith
}

void ExecutaFuncoes(void)
{
static int contF1 = 0;
if (F1exec){
if (contF1 < 5){
ExecutaComando(22); ExecutaComando(49);ExecutaComando(63);
}else if(contF1 < 6){
ExecutaComando(71);
ExecutaComando(71);ExecutaComando(71);ExecutaComando(71);
ExecutaComando(71);ExecutaComando(71);ExecutaComando(71);
ExecutaComando(71);
}else if(contF1 < 13){
ExecutaComando(28);
ExecutaComando(41);ExecutaComando(67);ExecutaComando(12);
ExecutaComando(39);ExecutaComando(53);
}else if(contF1 < 14){
ExecutaComando(79);
ExecutaComando(79);ExecutaComando(79);ExecutaComando(79);
ExecutaComando(79);ExecutaComando(79);ExecutaComando(79);
ExecutaComando(79);
}else if(contF1 < 15){
ExecutaComando(79);
ExecutaComando(79);ExecutaComando(79);ExecutaComando(79);
ExecutaComando(79);ExecutaComando(79);ExecutaComando(79);
ExecutaComando(79);
}else if(contF1 < 22){
ExecutaComando(22);
ExecutaComando(49);ExecutaComando(63);ExecutaComando(18);
ExecutaComando(31);ExecutaComando(57);
}else if(contF1 < 23){
ExecutaComando(71);
ExecutaComando(71);ExecutaComando(71);ExecutaComando(71);
ExecutaComando(71);ExecutaComando(71);ExecutaComando(71);
ExecutaComando(71);
}else if(contF1 < 24){
ExecutaComando(71);
ExecutaComando(71);ExecutaComando(71);ExecutaComando(71);
ExecutaComando(71);ExecutaComando(71);ExecutaComando(71);
ExecutaComando(71);
}else contF1 = 6;
contF1 ++;
}
}

void loop()
{
static int tempo = 9000; //variavel que define quantas passagens
corresponde a 1 tempo de execussao do robo(timer de execussao das
funcoes)
tempo --;
//Serial.println(tempo);
if (tempo == 0)
{
ExecutaFuncoes();
tempo = 200;
Serial.println("dentro");
}
if ( Serial.available())
{
char dado = Serial.read();
Serial.println(ExecutaComando(dado),DEC);
}//serial available
}