#define Itens_Menu 5
float MAX = 100.0 ,MIN = -100.0, Kp = 7.0, Kd = 10.0;
float vel8 = 0.0, vel9 = 0.0;
int cont8 = 0;
int cont9 = 0;
float power8, power9;
float var30 = 30.0;
float var90g = 7.0;
int velm1 = 100, velm2 = 93;
float tempo1 = 9.0 , tempo2 = 9.0;
int a1 = 0, a2 = 0, a3 = 0, a4 = 0, a5 = 0, a6 = 0, a7 = 0,a8=0;
int pid1, pid2, pid3, pid4, pid5, pidm1, pidm2,pid6,pid7,pid8;
int Parede_D = 14, Parede_E=4,Luz_H=2,Luz_V=6,Luz_Partida=9;
persistent int ponto_escuro = 100;
persistent int ponto_claro = 100;
persistent int media_cor[5][3];
persistent int dv[5][3];
persistent int VparedeD, VparedeE,pertoD,pertoE,longeD,longeE,Luz_Escolha;
char Menu[Itens_Menu][40] = {“0- Inicia_Partida Derick”, “1- Inicia_Partida2 Flavio”, “2- flavio calibrando”, “3- calibra tudo”, “4- polar”};
char bloco_cor[6][10] = {“Preto”, “Amarelo”, “azul”, “verde”, “vermelho”,”Erro”};
void velo8(){
int cont_ant = 0, te = 0;
vel8 = 0.0;
while(1){
if(cont8 != cont_ant){
vel8 = ((float)cont8 - (float)cont_ant)/((float)te*0.1);
cont_ant = cont8;
te = 1;
}
else {
te = te + 1;
if(te == 5) vel8 = 0.0;
}
sleep(0.1);
}
}
void velo9(){
int cont_ant2 = 0, te2 = 0;
vel9 = 0.0;
if(cont9 != cont_ant2){
vel9 = ((float)cont9 - (float)cont_ant2)/((float)te2*0.1);
cont_ant2 = cont9;
te2 = 1;
}
else {
te2 = te2 + 1;
if(te2 == 5) vel9 = 0.0;
}
} void count8(){
int est_ant = digital(8);
int est_ant2 = digital(7);
cont8 = 0;
cont9 = 0;
while(1){
if (est_ant == 0 && digital(8) == 1){
if(velm1>=0){
cont8 = cont8 + 1;
}
else {
cont8 = cont8 - 1;
}
}
est_ant = digital(8);
if (est_ant2 == 0 && digital(7) == 1){
if(velm2>=0){
cont9 = cont9 + 1;
}
else {
cont9 = cont9 - 1;
}
}
est_ant2 = digital(7);
sleep(0.004);
}
}
void start_menu(){
int i = 0;
int option;
while(1){
while((!start_button()) || (!stop_button())){
printf("%s\n",Menu[i]);
msleep(100L);
if (start_button()){
executeItem(i);
while (start_button());
}
if (stop_button()){
i = (i + 1)% Itens_Menu;
while (stop_button());
}
}
}
}
int rodar(float temp ,int vel, int mot){
motor(mot,vel); sleep(temp); off(mot);
} void stop(){
motor(0,0); motor(2,0); off(0); off(2);
}
void executa_teste(float ref){
int con;
float eps = 0.2;
while(!stop_button()){
con = cont8;
power8 = Kp*( ref - (float)con);
if(power8 > MAX){
power8= MAX;
}
else if(power8< MIN){
power8= MIN;
}
motor(0,(int)(power8+eps));
//printf("p = %f c = %d v = %f\n", power8, con, vel8);
}
}
void executa_teste2(float ref){
int con;
while(!stop_button()){
con = cont9;
power9 = Kd*( ref - (float)con);
if(power9 > MAX){
power9= MAX;
}
else if(power9< MIN){
power9= MIN;
}
motor(2,(int)power9);
}
}
int executa_andar(float ref1, float ref2){
int wall_D, cor;
float t1, t2;
cont8 = 0;
cont9 = 0;
executa_andar2(100);
wall_D = digital(Parede_D);
if (wall_D == 1){
executa_braco2();
wall_D= digital(Parede_D);
}
cont8 = 0;
cont9 = 0;
executa_andar2(-9);
rodar(0.4, -70, 3);
sleep(1.0);
off(3);
executa_andar2(-8);
executa_braco3();
executa_andar2(20);
wall_D = digital(Parede_D);
if (wall_D == 1){
wall_D= digital(Parede_D);
}
executa_braco();
cor=executa_cor();
//se nao preto: pega, da re ate a base
if(cor!=0){
executa_andar2(-40);
return 0;
}
else{
executa_andar2(-8);
executa_rodar(90);
executa_andar2(8);
executa_rodar(-90);
return 1;
}
stop();
}
void executa_andar_old(){
pidm1 = start_process(rodar(tempo1, velm1, 0)); pidm2 = start_process(rodar(tempo2, velm2, 2)); sleep(tempo1); kill_process(pidm1); off(0); kill_process(pidm2); off(2);
}
void executa_andar2(int ref){
int wall_D;
cont8 = 0;
cont9 = 0;
if (ref < 0){
velm1 = -velm1;
velm2 = -velm2;
pidm1 = start_process(rodar(30.0, velm1, 0));
pidm2 = start_process(rodar(30.0, velm2, 2));
}
else{
pidm1 = start_process(rodar(30.0, velm1, 0));
pidm2 = start_process(rodar(30.0, velm2, 2));
}
wall_D = digital(Parede_D);
if (ref < 0){
while(cont8 > ref){
}
}
else while(cont8 < ref && wall_D == 0){
wall_D = digital(Parede_D);
}
motor(0,0);
motor(2,0);
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
if (ref <= 0){
velm1 = -velm1;
velm2 = -velm2;
}
}
void execua_re(float tempo){
pidm1 = start_process(rodar(tempo, -70, 0)); pidm2 = start_process(rodar(tempo, -70, 2)); sleep(tempo); kill_process(pidm1); off(0); kill_process(pidm2); off(2);
}
void executa_rodar(int graus){
float temp1;
if(graus==90||graus==-90){
temp1=2.3;
}else{
if(graus==15){
temp1=0.3;
}else{
temp1=4.5;
}
}
if(graus>0){
pidm1 = start_process(rodar(temp1, -velm1, 0));
pidm2 = start_process(rodar(temp1, velm2, 2));
}else{
pidm1 = start_process(rodar(temp1, velm1, 0));
pidm2 = start_process(rodar(temp1, -velm2, 2));
}
sleep(temp1);
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
}
Calibra linha escura e clara
void calibra_linha(){
int new;
int cont=0;
int soma=0;
while(cont<10) {
new = sensor_linha();
printf("Linha escura=%d\n",new);
sleep(0.2);
soma =soma+new;
sleep(0.2);
cont++;
}
ponto_escuro=soma/10;
printf("Linha escura=%d\n",ponto_escuro);
sleep(1.0);
printf("Start para ponto branco");
while(!start_button()){
}
cont=0;
soma=0;
while(cont<10) {
new = sensor_linha();
printf("Linha clara=%d\n",new);
sleep(0.2);
soma =soma+new;
sleep(0.2);
cont++;
}
ponto_claro=soma/10;
printf("Linha clara=%d\n", ponto_claro);
sleep(1.0);
}
void calibra_cor(){
int new;
int soma=0;
int i, j, k, maior = 0, menor = 256;
poke(0x1009, 0x3c);
for (i = 0; i < 5;i ++){
sleep(0.5);
printf("Start para %s\n",bloco_cor[i]);
while(!start_button()){
}
for (j = 0; j < 3; j ++){
soma = 0;
for (k = 0; k < 2; k ++){
acende_led(j);
sleep(0.2);
new = sensor_cor();
if (new > maior) maior = new;
if (new < menor) menor = new;
printf("%s = %d\n", bloco_cor[i], new);
soma = soma + new;
sleep(0.1);
}
apaga_led(j);
media_cor[i][j] = soma/2;
dv[i][j] = maior- menor;
}
}
}
int executa_cor(){
int i,j,k,numCor=5;
int cor[3];
int soma,new;
printf("Executando leitura\n");
sleep(0.5);
for (j = 0; j < 3; j ++){
soma = 0;
for (k = 0; k < 3; k ++){
acende_led(j);
sleep(0.2);
new = sensor_cor();
soma = soma + new;
sleep(0.1);
}
apaga_led(j);
cor[j]=soma/3;
}
for(i=0;i<5;i++){
if(cor[0]>=(media_cor[i][0]-4)&&cor[0]<=(media_cor[i][0]+4)&&cor[1]>=(media_cor[i][1]-4)&&cor[1]<=(media_cor[i][1]+4)&&cor[2]>=(media_cor[i][2]-4)&&cor[2]<=(media_cor[i][2]+4)){
numCor=i;
}
}
printf("Cor= %s\n", bloco_cor[numCor]);
sleep(0.5);
if(numCor==5){
return(0);
}else{
return(numCor);
}
}
Executa andar na linha
void executa_linha(){
while(1) {
andar_direita();
na_linha();
fora_linha();
para_motor();
sleep(0.1);
andar_esquerda();
na_linha();
fora_linha();
}
}
motor direita: 2 motor esquerda:0
void andar_reto(){
motor(0,95); motor(2,95);
}
void andar_esquerda(){
//motor(0,0); //motor(2,90); //sleep(0.5); motor(0,30); motor(2,90);
}
void andar_direita(){
//motor(0,80); //motor(2,0); //sleep(0.4); motor(0,75); motor(2,20);
}
void para_motor(){
motor(0,0); motor(2,0);
}
Calibra Parede
void calibra_parede(){
int wall= analog(Parede_E);
sleep(1.0);
while(!start_button()){
wall= analog(Parede_E);
printf("Start para Parede_E= %d\n",wall);
sleep(0.5);
}
printf("Parede_E= %d\n",wall);
sleep(1.0);
VparedeE= wall;
beep();
sleep(1.0); /* give a pause for the display */
wall= digital(Parede_D);
while(!start_button()){
wall= digital(Parede_D);
printf("Start para Parede_D= %d\n",wall);
sleep(0.5);
}
printf("Parede_D= %d\n",wall);
sleep(1.0);
VparedeD= wall;
beep();
printf("Fim da calibragem\n");
sleep(1.0); /* give a pause for the display */
}
void calibra_paredeGentle(){
int wall= analog(Parede_E);
while(!start_button()){
wall= analog(Parede_E);
printf("Start para Parede_E perto= %d\n",wall);
sleep(0.5);
}
pertoE= wall;
printf("pertoE %d\n", pertoE);
beep();
sleep(1.0);
while(!start_button()){
wall= analog(Parede_E);
printf("Start para Parede_E longe= %d\n",wall);
}
longeE= wall;
printf("longeE %d\n", longeE);
beep();
sleep(1.0);
//parede da direita//
while(!start_button()){
wall= digital(Parede_D);
printf("Start para Parede_D perto= %d\n",wall);
}
pertoD= wall;
printf("pertoD %d\n", pertoD);
beep();
sleep(1.0);
while(!start_button()){
wall= digital(Parede_D);
printf("Start para Parede_D longe= %d\n",wall);
}
longeD= wall;
printf("longeD %d\n", longeD);
beep();
sleep(1.0);
printf("Fim da Calibracao.\n");
beep();
sleep(0.5);
}
Segue parede simples void Parede_Simples(){
//x= 0;
while (1) {
int wall_D=digital(Parede_D);
int wall_E=analog(Parede_E);
printf("wall_D:%d wall_E: %d\n", wall_D, wall_E);
if (wall_E > VparedeE) andar_esquerda(); /*longe */
else andar_direita(); /* perto */
if (wall_D < VparedeD){
execua_re(1.3);
executa_rodar(90); /* longe */
}
}
}
Segue parede Gentle void Parede_Gentle(){
while (1) {
//int x =0;
int wall_D= digital(Parede_D);
int wall_E=analog(Parede_E);
printf("wall_D:%d wall_E: %d\n", wall_D, wall_E);
if (wall_E >= longeE){ /* too far */
andar_esquerda();
}else{
if (wall_E <= pertoE){ /* too close */
andar_direita();
}else{
if(wall_D >= longeD){
andar_reto();
}
}
}
if (wall_D < longeD){ /* too close */
execua_re(1.3);
executa_rodar(90);
}
}
}
void alinhar(){
if(Luz_Escolha==Luz_H){
int luzh = digital(Luz_H);
int luzv = digital(Luz_V);
}else{
int luzv = digital(Luz_H);
int luzh = digital(Luz_V);
}
if(!luzv() && luzh()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
}else{
pidm1 = start_process(rodar(10.0, -velm1, 0));
pidm2 = start_process(rodar(10.0, velm2, 2));
while(1){
if(!luzv() && luzh()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
break;
}
//printf("luz:%d%d\n", luzv, luzh);
sleep(0.5);
}
}
beep();
sleep(0.5);
}
void alinhar2(){
if(Luz_Escolha==Luz_H){
int luzh = digital(Luz_H);
int luzv = digital(Luz_V);
}else{
int luzv = digital(Luz_H);
int luzh = digital(Luz_V);
}
if(!luzv2() && luzh2()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
}else{
pidm1 = start_process(rodar(10.0, -velm1, 0));
pidm2 = start_process(rodar(10.0, velm2, 2));
while(1){
if(!luzv2() && luzh2()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
break;
}
sleep(0.5);
}
}
beep();
sleep(0.5);
}
void escohe_luzV(){
Luz_Escolha=Luz_V;
printf("Vertical");
sleep(1.5);
}
void escohe_luzH(){
Luz_Escolha=Luz_H;
printf("Horizonal");
sleep(1.5);
}
void na_linha(){
while(sensor_linha() < ponto_escuro);
}
void fora_linha(){
while(sensor_linha() > ponto_claro);
}
int sensor_linha(){
return analog(2);
} int sensor_cor(){
return analog(3);
}
void executa_reta(){
executa_andar(var30, var30); executa_andar(var90g,-var90g); executa_andar(var90g,-var90g); executa_andar(var30, var30);
}
void acende_led(int cor){
if (cor == 0){
bit_set(0x1008, 4);//led vermelho
}
else if(cor == 1){
bit_set(0x1008, 8);//led verde
}
else if(cor == 2){
bit_set(0x1008, 16);//led amarelo
}
}
void apaga_led(int cor){
if (cor == 0)
bit_clear(0x1008, 4);
else if(cor == 1)
bit_clear(0x1008, 8);
else if(cor == 2)
bit_clear(0x1008, 16);
}
void executa_luzes(){
int tempo;
tempo=0;
while(tempo<8){
bit_set(0x1008, 4);//led vermelho//
sleep(1.0);
bit_clear(0x1008, 4);
bit_set(0x1008, 8);//led verde//
sleep(1.0);
bit_clear(0x1008, 8);
bit_set(0x1008, 16);//led amarelo//
sleep(1.0);
bit_clear(0x1008, 16);
tempo++;
}//end while//
}
void executa_musica2(){
}
void executa_musica(){
int pidt1, pidt2,pidt3; pidt1 = start_process(executa_luzes()); pidt2 = start_process(rodar(20.0 ,50, 0)); pidt3 = start_process(executa_musica2()); sleep(20.0); motor(0,0); off(0); kill_process(pidt2); kill_process(pidt3); tone(380.0, 0.01); bit_clear(0x1008, 4); bit_clear(0x1008, 8); bit_clear(0x1008, 16); kill_process(pidt1);
}
int andar_frente(){
int pidteste2 = start_process(count8()); int reu; reu = executa_andar(30.0, 30.0); kill_process(pidteste2); return (reu);
}
int naobate(){
while (1) {
int wall_D= digital(Parede_D);
int wall_E=analog(Parede_E);
printf("wall_D:%d wall_E: %d\n", wall_D, wall_E);
if (wall_E <= pertoE){ /* too close */
andar_direita();
}else{
if(wall_D == 0){
andar_reto();
}
}
if (wall_D == 1 || sensor_cor()>240){ /* too close */
stop();
break;
}
}
}
void voltabase(){
execua_re(1.5);
while(1){
if (analog(Luz_Escolha)<50){
break;
}else{
execua_re(1.5);
alinhar2();
}
}
}
void tatica1(){
int cor=5;
while(1){
naobate();
//verificacor//
cor=executa_cor();
//se nao preto: pega, da re ate a base//
if(cor!=0){
executa_braco();
execua_re(2.0);
voltabase();
executa_rodar(90);
executa_braco3();
executa_rodar(-90);
}else{//se preto: vira a direita e não bate. //
execua_re(1.3);
executa_rodar(-90);
}
}
}
void executa_taticas(int p){
int reu = 1;
if(p == 1){
tatica1();
}
else{
while(reu == 1){
reu = andar_frente();
}
}
}
void begin(int p){
calibra_luz(); executa_taticas(p);
} int luzv(){
int luzv = analog(Luz_V);
if(luzv<=35){
return(1);
}else{
return(0);
}
} int luzh(){
int luzh = analog(Luz_H);
if(luzh<=75){
return(1);
}else{
return(0);
}
} void calibra_luz(){
if(!luzv() && luzh()){
off(0);
off(2);
Luz_Escolha=Luz_H;
}else if(luzv() && !luzh()){
off(0);
off(2);
Luz_Escolha=Luz_V;
}else{
pidm1 = start_process(rodar(10.0, -velm1, 0));
pidm2 = start_process(rodar(10.0, velm2, 2));
while(1){
if(!luzv() && luzh()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
Luz_Escolha=Luz_H;
break;
}else if(luzv() && !luzh()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
Luz_Escolha=Luz_V;
break;
}else{
sleep(1.0);
}
}
}
}
int luzv2(){
int luzv = analog(Luz_V);
if(luzv<=100){
return(1);
}else{
return(0);
}
} int luzh2(){
int luzh = analog(Luz_H);
if(luzh<=150){
return(1);
}else{
return(0);
}
}
void calibra_luz2(){
if(!luzv2() && luzh2()){
off(0);
off(2);
Luz_Escolha=Luz_H;
}else if(luzv2() && !luzh2()){
off(0);
off(2);
Luz_Escolha=Luz_V;
}else{
pidm1 = start_process(rodar(10.0, -velm1, 0));
pidm2 = start_process(rodar(10.0, velm2, 2));
while(1){
if(!luzv2() && luzh2()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
Luz_Escolha=Luz_H;
break;
}else if(luzv2() && !luzh2()){
kill_process(pidm1);
off(0);
kill_process(pidm2);
off(2);
Luz_Escolha=Luz_V;
break;
}else{
sleep(1.0);
}
}
}
}
void Calibra_tudo(){
calibra_cor(); calibra_paredeGentle();
}
void inicia_Partida2(int parametro){
int luz= analog(5);
poke(0x1009, 0x3c);
while(1){
if(luz < 5){
int pidP = start_process(begin(parametro));
sleep(60.0);
stop();
off(3);
break;
}
luz= analog(5);
printf("Esperando parida %d\n",luz);
sleep(0.2);
}
} void inicia_Partida(int parametro){
int luz= analog(5);//digital(Luz_Partida);
int calibra_cor=0;
poke(0x1009, 0x3c);
sleep(1.5);
while(1){
printf("Start-Caibra Stop-No\n" );
if(!stop_button()){
calibra_cor=1;
break;
}else if(!start_button()){
break;
}
}
if(calibra_cor==1){
Calibra_tudo();
}
while(1){
if(luz < 5){
int pidP = start_process(begin(parametro));
sleep(60.0);
stop();
motor(3,0);
off(3);
break;
}
luz= analog(5);//digital(Luz_Partida);
printf("Esperando parida %d\n",luz);
sleep(0.2);
}
}
void executa_braco(){
rodar(0.8, -70, 3); sleep(1.0); off(3); off(3);
}
void executa_braco2(){
rodar(0.65, -70, 3); sleep(0.8); off(3); off(3);
}
void executa_braco3(){
rodar(0.65, 85, 3); sleep(1.0); off(3);
}
void polar(){
int luzv,luzh,ldr;
while(!stop_button()){
ldr=sensor_cor();
luzv = analog(Luz_V);
luzh= analog(Luz_H);
printf("luz:V:%dH:%d %d\n", luzv, luzh, ldr);
sleep(1.0);
}
}
void executeItem(int MenuOption){
int lixo = 1;
if (MenuOption == 0){
inicia_Partida2(2);
}
if (MenuOption == 1){
inicia_Partida2(1);
}
if( MenuOption == 2){
inicia_Partida(2);
}
if( MenuOption == 3){
Calibra_tudo();
}
if( MenuOption == 4){
polar();
}
}