Você está na página 1de 11

//Declarando Variáveis

int numero = 0;
float correcao = 0;
float angulo = 0;
float erro = 0;
float erro2 = 0;

float kp = 20;
float ki = 0.000001f;
float kd = 5;

float integral = 0;
float derivada = 0;
float lasterro = 0;
float dis1 = 0;
float dis2 = 0;

float segundos = 0;

float vel = 200;


float velDireita = 0;
float velEsquerda = 0;

float branco = 50;


float preto = 15;

float verde = 50;


float vermelho = 33;
float azul = 22;

float distancia = 0;

bool a = true;
bool t = true;

int seg = 50;


int l = 1;

float constante = 1;

int i = 0;
int j = 0;

float start = 0;
float end = 0;
float tempoTotal = 0;

int cont = 0;

int indice = 0;

int grauFrente = 6;
int grau = 1;

String S1 = "";
String S2 = "";
String S3 = "";
String S4 = "";
String S5 = "";
String A = "";

bool rampa_vari = true;

int[] leitura = new int[5];

bool[] posicao = new bool[8];

float media = 0;

int numeroLinha = 0;
int contagemLinha = 13;

//Funções Diversas
Func<int> direcao = () => (int)bc.compass();
Action<int> esquerda = (x) => bc.onTF(x, -x);
Action<int> direita = (x) => bc.onTF(-x, x);
Func<int, float> sensor = (x) => (float)bc.lightness(x);
Action<int> motores = (x) => bc.onTF(x, x);
Action parar = () => bc.onTF(0f, 0f);
Action<int> esperar = (x) => bc.wait(x);

Action garra = () => {


bc.onTF(-100, -100);
esperar(500);
while(bc.angleActuator() < 290 || bc.angleActuator() > 290){
bc.actuatorUp(seg);
}
};

bc.actuatorSpeed(150);
garra();
//Ângulo Ajuste
Action anguloAjuste = () => {
//Primeiro Quadrante
if((direcao() > 0 || direcao() < 360) && direcao() <= 45){
angulo = direcao();
bc.onTFRot(500, -angulo);
}
//Segundo Quadrante
else if(direcao() > 45 && direcao() <= 90){
angulo = 90 - direcao();
bc.onTFRot(500, angulo);
}
//Terceiro Quadrante
else if(direcao() > 90 && direcao() <= 135){
angulo = 90 - direcao();
bc.onTFRot(500, angulo);
}
//Quarto Quadrante
else if(direcao() > 135 && direcao() <= 180){
angulo = 180 - direcao();
bc.onTFRot(500, angulo);
}
//Quinto Quadrante
else if(direcao() > 180 && direcao() <= 225){
angulo = 180 - direcao();
bc.onTFRot(500, angulo);
}
//Sexto Quadrante
else if(direcao() > 225 && direcao() <= 270){
angulo = 270 - direcao();
bc.onTFRot(500, angulo);
} //Sétimo Quadrante
else if(direcao() > 270 && direcao() <= 315){
angulo = 270 - direcao();
bc.onTFRot(500, angulo);
} //Oitava Quadrante
else if(direcao() > 315 && direcao() <= 360){
angulo = 360 - direcao();
bc.onTFRot(500, angulo);
}
};

Action calibracao = () => {

//Primeiro Quadrante
if((direcao() > 0 || direcao() < 360) && direcao() <= 45){
posicao[0] = true;
}
//Segundo Quadrante
else if(direcao() > 45 && direcao() <= 90){
posicao[1] = true;
}
//Terceiro Quadrante
else if(direcao() > 90 && direcao() <= 135){
posicao[2] = true;
}
//Quarto Quadrante
else if(direcao() > 135 && direcao() <= 180){
posicao[3] = true;
}
//Quinto Quadrante
else if(direcao() > 180 && direcao() <= 225){
posicao[4] = true;
}
//Sexto Quadrante
else if(direcao() > 225 && direcao() <= 270){
posicao[5] = true;
}
//Sétimo Quadrante
else if(direcao() > 270 && direcao() <= 315){
posicao[6] = true;
} //Oitava Quadrante
else if(direcao() > 315 && direcao() <= 360){
posicao[7] = true;
}
//condição para fazer a calibração
if(posicao[0] == true || posicao[7] == true){
//anguloAjuste();
for(i=0;i<5;i++){
media += sensor(i);
}
}
else if(posicao[1] == true || posicao[2] == true){
//anguloAjuste();
for(i=0;i<5;i++){
media += sensor(i);
}
}
else if(posicao[3] == true || posicao[4] == true){
//anguloAjuste();
for(i=0;i<5;i++){
media += sensor(i);
}
}
else if(posicao[5] == true || posicao[6] == true){
//anguloAjuste();
for(i=0;i<5;i++){
media += sensor(i);
}
}
media /= 5;
bc.printLCD(1, media.ToString());
};

Action buscarLinha = () => {


//while(a == true){
bc.turnLedOn("BRANCO");
i=0; numeroLinha = 1;
while(sensor(0) >= branco && sensor(1) >= branco && sensor(2) >= branco &&
sensor(3) >= branco && sensor(4) >= branco){
bc.onTFRot(500, i);
i++;
bc.printLCD(3, i.ToString());
if(i >= contagemLinha){
numeroLinha = 2;
bc.onTFRot(500, -95);
i = 0;
while(sensor(0) >= branco && sensor(1) >= branco && sensor(2) >=
branco && sensor(3) >= branco && sensor(4) >= branco){
bc.onTFRot(500, -i);
i++;
if(i >= contagemLinha){
numeroLinha = 3;
bc.onTFRot(500, 50);
while(sensor(0) >= branco && sensor(1) >= branco &&
sensor(2) >= branco && sensor(3) >= branco && sensor(4) >= branco){
motores(-150);
}
}
}
}
}
//}
};

Action curva90 = () => {


if(sensor(0) < preto && sensor(1) < preto){
parar();
bc.onTFRotations(200, 14);
parar();
while(sensor(0) >= branco && sensor(1) >= branco && sensor(2) >= branco &&
sensor(3) >= branco && sensor(4) >= branco){
bc.onTFRot(500, grau);
esperar(1);
motores(-100);
esperar(10);
}
//motores(160);
//esperar(200);
motores(-100);
esperar(250);
//anguloAjuste();
}
else if(sensor(3) < preto && sensor(4) < preto){
parar();
bc.onTFRotations(200, 14);
parar();
while(sensor(0) >= branco && sensor(1) >= branco && sensor(2) >= branco &&
sensor(3) >= branco && sensor(4) >= branco){
bc.onTFRot(500, -grau);
esperar(1);
motores(-100);
esperar(10);
}
//motores(160);
//esperar(200);
motores(-100);
esperar(250);
//anguloAjuste();
}
};

Action Green = () => {


if((bc.returnRed(0) <= vermelho && bc.returnGreen(0) >= verde &&
bc.returnBlue(0) <= azul) || (bc.returnRed(1) <= vermelho && bc.returnGreen(1) >=
verde && bc.returnBlue(1) <= azul)){
parar();
esperar(400);
if((bc.returnRed(3) <= vermelho && bc.returnGreen(3) >= verde &&
bc.returnBlue(3) <= azul) || (bc.returnRed(4) <= vermelho && bc.returnGreen(4) >=
verde && bc.returnBlue(4) <= azul)){
bc.onTFRot(300, 180);
}
else{
bc.turnLedOn("VERDE");
parar();
bc.onTFRotations(300, 16);
parar();
bc.onTFRot(500, 25);
while(sensor(2) > branco){
bc.onTFRot(500, grau);
}
//bc.onTFRot(500, 5);
motores(-150);
esperar(200);
//anguloAjuste();
}
}
else if((bc.returnRed(3) <= vermelho && bc.returnGreen(3) >= verde &&
bc.returnBlue(3) <= azul) || (bc.returnRed(4) <= vermelho && bc.returnGreen(4) >=
verde && bc.returnBlue(4) <= azul)){
parar();
esperar(400);
if((bc.returnRed(0) <= vermelho && bc.returnGreen(0) >= verde &&
bc.returnBlue(0) <= azul) || (bc.returnRed(1) <= vermelho && bc.returnGreen(1) >=
verde && bc.returnBlue(1) <= azul)){
bc.onTFRot(300, 180);
}
else{
bc.turnLedOn("VERDE");
parar();
bc.onTFRotations(300, 16);
parar();
bc.onTFRot(500, -25);
while(sensor(2) > branco){
bc.onTFRot(500, -grau);
}

//bc.onTFRot(500, -5);
motores(-150);
esperar(200);
//anguloAjuste();
}
}
};

Action FollowLine = () => {


erro = (bc.lightness(0) + bc.lightness(1))-(bc.lightness(3) + bc.lightness(4));

//erro = sensor(0) - sensor(1);


//erro *= 1.3f;
bc.printLCD(1, erro.ToString());
if(erro > 30){
bc.onTFRot(500, -grau);
esperar(1);
motores(-100);
esperar(10);
}
else if(erro < -30){
bc.onTFRot(500, grau);
esperar(1);
motores(-100);
esperar(10);
}
else{
bc.onTF(vel, vel);
}
//bc.onTFRot(500, erro);
//bc.printLCD(1, erro.ToString());
};

Action desviar = () => {


if(bc.distance(0) <= 20 || bc.distance(2) <= 20 && bc.returnColor(5) !=
"BRANCO"){
//anguloAjuste();
bc.turnLedOn("PRETO");
bc.onTFRot(400, 45);
bc.onTF(100, 100);
esperar(1900);
bc.onTFRot(400, -45);
anguloAjuste();
bc.onTF(130, 130);
esperar(1100);
bc.onTFRot(400, -45);
bc.onTF(100, 100);
esperar(1100);
motores(200);
esperar(400);
bc.onTFRot(400, 40);
anguloAjuste();
while(bc.touch(0) != true){
motores(-150);
}
}
};

Action sensores = () => {


S1 = "Luz 1 = " + sensor(0).ToString();
S2 = ", Luz 2 = " + sensor(1).ToString();
String S7 = ", Luz 3 = " + sensor(2).ToString();
S3 = ", Luz 4 = " + sensor(3).ToString(); S4 = ", Luz 5 = " +
sensor(4).ToString();
S5 = S1 + S2; String S6 = S7 + S3; A = S5 + S6;
A += S4; bc.printLCD(1,A);
};

Action devolver = () => {


bc.onTFRot(500, 90);
//Indo até o Triângulo
while(bc.returnColor(5) != "PRETO"){
motores(100);
}
parar();
esperar(100);
bc.turnActuatorUp(500);
bc.actuatorDown(200);
parar();
esperar(500);
while(bc.angleActuator() < 290 || bc.angleActuator() > 290){
bc.actuatorUp(seg);
}
//Voltando para buscar
while(bc.angleBucket() != 1 && bc.angleBucket() != 3 && bc.angleBucket() != 5
&& bc.angleBucket() != 2 && bc.angleBucket() != 357 ){
if(bc.angleBucket() <= 300){
bc.turnActuatorUp(seg);
}
else{
bc.turnActuatorDown(seg);
}
bc.printLCD(2, bc.angleBucket().ToString());
}
parar();
esperar(100);
motores(-150);
esperar(200);
anguloAjuste();
bc.onTFRot(300, -180);
anguloAjuste();
};

Action buscarBola = () => {


//int area = (bc.distance(0) >= 185) ? 1 : 0;
//Calculando a distância da bola em relação a parede
distancia = 245 - bc.distance(1);
if(bc.distance(1) <= 35){
bc.onTFRot(300, 90);
bc.onTF(100, 100);
esperar(1200);
while(bc.touch(0) != true){
motores(-100);
}
}
else{
bc.onTFRot(300, 90);
}
parar();
esperar(100);
bc.actuatorDown(1000);
//Aqui ve a bola
if(distancia <= 30){
while(bc.distance(0) >= 30){
motores(500);
}
}
else{
while(distancia <= bc.distance(0)){
motores(500);
}
}
esperar(100);
parar();
esperar(200);
bc.actuatorUp(550);
bc.turnActuatorUp(100);
parar();
esperar(650);
while(bc.touch(0) != true){
bc.onTFRotations(500, -1);
}
motores(100);
esperar(150);
if(bc.hasVictims() == true){
devolver();
}
else{
bc.onTFRot(500, -90);
while(bc.angleActuator() < 290 || bc.angleActuator() > 290){
bc.actuatorUp(seg);
}
//Voltando para buscar
while(bc.angleBucket() != 1 && bc.angleBucket() != 3 && bc.angleBucket() !=
5 && bc.angleBucket() != 2 && bc.angleBucket() != 357 ){
if(bc.angleBucket() <= 300){
bc.turnActuatorUp(seg);
}
else{
bc.turnActuatorDown(seg);
}
//bc.printLCD(2, bc.angleBucket().ToString());
}
motores(-150);
esperar(500);
}
};

Action resgatar = () => {


//Saindo do Triângulo
while(bc.distance(0) >= 16){
motores(500);
}
//Alinhando para fazer a varredura
bc.onTFRot(300, 15);
anguloAjuste();
//Varredura
while(true){
//Buscando Vítimas no lado Esquerdo
if(bc.distance(1) < 245){
motores(100);
esperar(300);
buscarBola();
}
else{
bc.printLCD(1, "BUSCANDO VITIMA...");
motores(200);
}
}
};

Action triangulo = () => {


bc.actuatorSpeed(95);
anguloAjuste();
while(bc.distance(0) >= 10){
if(bc.returnColor(5) != "PRETO"){
bc.onTF(200,200);
}
else{
motores(100);
esperar(150);
if(sensor(5) <= 13){
bc.onTFRotations(200, 4);
if(bc.returnColor(5) == "PRETO"){
for(i = 1; i <= 3; i++){
if(l == i){
parar();
esperar(500);
anguloAjuste();
motores(-100);
esperar(100);
bc.onTFRot(300, 40);
resgatar();
}
}
}
}
else{
motores(-100);
esperar(200);
bc.actuatorDown(1000);
motores(200);
esperar(400);
parar();
esperar(200);
bc.turnActuatorDown(500);
bc.actuatorUp(550);
}
}
}
l++;
motores(-50);
esperar(300);
anguloAjuste();
bc.onTFRot(300, 90);
anguloAjuste();
};

Action rampa = () => {


bc.actuatorDown(350);
anguloAjuste();
while(true){
if(bc.distance(1) <= 40){
motores(200);
// FollowLine();
}
else{
motores(100);
esperar(500);
anguloAjuste();
garra();
while(true){
triangulo();
}
}
}
};

Action buscar = () => {


if(sensor(0) >= branco && sensor(1) >= branco && sensor(2) >= branco &&
sensor(3) >= branco && sensor(4) >= branco){
bc.resetTimer(); bc.printLCD(1, "TO VENDO BRANCO...");
while(bc.timer() <= 1100 && sensor(0) >= branco && sensor(1) >= branco &&
sensor(2) >= branco && sensor(3) >= branco && sensor(4) >= branco){
FollowLine();
}
if(bc.timer() >= 1100){
buscarLinha();
}
//bc.printLCD(2, bc.timer().ToString());
}
else{
bc.resetTimer();
}
};

Action rampas = () => {


while(rampa_vari == false){
if(bc.inclination() > 300 && bc.distance(1) >= 30 && bc.distance(2) >= 30){

while(bc.angleActuator() > 0){


bc.actuatorDown(seg);
}
FollowLine();
}
else{
rampa_vari = true;
}
}
parar();
esperar(100);
};

while(true){
dis1 = bc.distance(0);
dis2 = bc.distance(2);
//bc.changePencilColor("AMARELO");
bc.turnLedOn("VERMELHO");
desviar();
FollowLine();
Green();
curva90();
if(bc.distance(1) <= 30){
motores(100);
esperar(270);
rampa();
}
buscar();
//calibracao();
//sensores();
bc.printLCD(2, bc.distance(0).ToString());
bc.printLCD(3, bc.distance(2).ToString());

Você também pode gostar