Gabriel Patrocinio
/
PET-Finder_Final
Projeto Mecatrônico - Pick and Place
main.cpp
- Committer:
- gabrielpp
- Date:
- 2017-06-06
- Revision:
- 0:366afb0a6154
File content as of revision 0:366afb0a6154:
#include "mbed.h" #include "TextLCD.h" Ticker Teclado; Timer t; //Constantes #define maxSpeed 7000 //Variaveis int tempInt; int inputX; int inputY; int inputZ; int i,j,k; int type_1=0; int type_2=0; int type_3=0; int currentX = 0; int currentY = 0; int currentZ = 0; int grip_state=-1; int cs; int times = 500; int xMax = -1; int yMax = -1; int zMax = -1; int selection; int stopParam = 0; int accelParam = 1; int acceleration = 5; int speedLimit = 1000; int pickCoords[4]; int placeCoords1[4]; int placeCoords2[4]; int placeCoords3[4]; float currentXmm = 0; float currentYmm = 0; float currentZmm = 0; float tempFloat; float inputXmm; float inputYmm; float inputZmm; float pulse_mm = 1.5/200; float mm_pulse = 200/1.5; float vertical; float horizontal; float red = 0; float green = 0; float blue = 0; float fodase; float redm; float greenm; float bluem; float r[501]; float g[501]; float b[501]; char tempChar; char rgb=' '; char characters_teclado[] = {'1','2','3','x','4','5','6','y','7','8','9','z','d','0','u','e'}; char user_inputX[64] = {'+','0','0','0','.','0','0','0'}; char user_inputY[64] = {'+','0','0','0','.','0','0','0'}; char user_inputZ[64] = {'+','0','0','0','.','0','0','0'}; InterruptIn FDC(PC_5); //Motores DigitalOut pwmX(PA_10); DigitalOut dirX(PB_4); DigitalOut pwmY(PB_3); DigitalOut dirY(PB_10); DigitalOut pwmZ(PB_5); DigitalOut dirZ(PA_8); DigitalOut motorEnable(PA_9); //Buzzer DigitalOut Buzzer(PA_15); //LCD TextLCD lcd(PC_4,PB_1,PB_11,PB_12,PA_11,PA_12,TextLCD::LCD20x4); //Garra DigitalOut close_grip_var(PC_7); DigitalOut enable_grip(PA_7); DigitalOut open_grip_var(PA_6); //Teclado BusIn Cs(PA_13,PB_0,PD_2,PB_2); BusOut Ls(PC_2,PC_3,PC_9,PC_12); //Sensores DigitalIn capacitive(PB_6); DigitalIn out(PC_6); DigitalOut s2(PB_15); DigitalOut s3(PC_8); //Portas Analogicas AnalogIn potenciometro_velocidade(PA_0); //A0 AnalogIn xaxis(PA_1); //D1 AnalogIn yaxis(PA_4); //D0 //Funcoes de conversao void getPosmm(){ currentXmm=pulse_mm*currentX; currentYmm=pulse_mm*currentY; currentZmm=pulse_mm*currentZ; } void ConvertmmPulse(float posxmm,float posymm, float poszmm){ inputX = posxmm*mm_pulse; inputY = posymm*mm_pulse; inputZ = poszmm*mm_pulse; } void ConvertPulsemm(int posx,int posy, int posz){ inputXmm = posx*pulse_mm; inputYmm = posy*pulse_mm; inputZmm = posz*pulse_mm; } //Funções da Garra void open_grip(){ enable_grip=1; open_grip_var=1; close_grip_var=0; wait_ms(800); grip_state=0; enable_grip=0; open_grip_var=0; } void close_grip(){ enable_grip=1; open_grip_var=0; close_grip_var=1; wait_ms(800); grip_state=1; enable_grip=0; close_grip_var=0; } //Funções RGB int pulseIn(int state){ while(out != state){ } t.reset(); t.start(); while(out == state){ } int period = t.read_us(); t.stop(); return period; } void rotina_rgb(){ int count = 0; char oldRGB = ' '; for(int i=0;i<times;i++){ s2=1; s3=1; fodase = pulseIn(1); s2=0; s3=0; red = pulseIn(1); s3=1; blue= pulseIn(1); s2=1; green= pulseIn(1); r[i] = red; g[i] = green; b[i] = blue; } while(count < 51){ s2=1; s3=1; fodase = pulseIn(1); s2=0; s3=0; red = pulseIn(1); s3=1; blue= pulseIn(1); s2=1; green= pulseIn(1); for(int i=1;i<times;i++){ r[i] = r[i-1]; g[i] = g[i-1]; b[i] = b[i-1]; } r[0] = red; g[0] = green; b[0] = blue; redm = 0; greenm = 0; bluem = 0; for(int i=0;i<times;i++){ redm = redm + r[i]; greenm = greenm + g[i]; bluem = bluem + b[i]; } redm = redm/times; greenm = greenm/times; bluem = bluem/times; if(abs(350-redm) < 50 && abs(350-greenm) < 50 && abs(350-bluem) < 50){ rgb = 'r'; }else if(abs(450-redm) < 50 && abs(450-greenm) < 50 && abs(450-bluem) < 50){ rgb = 'a'; }else{ rgb = ' '; } if(rgb == 'r'){ if(rgb != oldRGB){ count = 0; } count++; }else if(rgb == 'a'){ if(rgb != oldRGB){ count = 0; } count++; }else{ count = 0; } } } //Função do Teclado void checkTeclado(){ tempChar=' '; for (int ti = 0; ti<4; ti++){ Ls = 1 << ti; switch(Cs){ case 0x1: tempChar = characters_teclado[4*ti]; break; case 0x2: tempChar = characters_teclado[4*ti+1]; break; case 0x4: tempChar = characters_teclado[4*ti+2]; break; case 0x8: tempChar = characters_teclado[4*ti+3]; break; default: break; } } Ls = 0; } /*---------------Funções de Motores---------------*/ //Move void move(int motor, int direction, int pulse){ if(motor == 1){ dirX = direction; int accelTime; int delay; int speed; if(accelParam == 0){ speed = potenciometro_velocidade.read()*speedLimit; accelTime = 0; }else{ speed = 1; accelTime = (potenciometro_velocidade.read()*speedLimit)/acceleration; } for(int i=0;i<pulse;i++){ if(stopParam){ dirX = !dirX; currentX = currentX - 3; for(int j=0;j<3;j++){ pwmX = 1; wait_us(350); pwmX = 0; wait_us(350); } break; }else{ if(speed == 0){ while(speed == 0){ speed = potenciometro_velocidade.read()*speedLimit; wait_ms(100); } } delay = (1000000/speed)/2; pwmX = 1; wait_us(delay); pwmX = 0; wait_us(delay); if(direction == 0){ currentX++; }else{ currentX--; } if(i < accelTime){ speed = speed + acceleration; }else if(i > (pulse - accelTime)){ speed = speed - acceleration; }else{ speed = potenciometro_velocidade.read()*speedLimit; accelTime = speed/acceleration; } } } }else if(motor == 2){ dirY = direction; int accelTime; int delay; int speed; if(accelParam == 0){ speed = potenciometro_velocidade.read()*speedLimit; accelTime = 0; }else{ speed = 1; accelTime = (potenciometro_velocidade.read()*speedLimit)/acceleration; } for(int i=0;i<pulse;i++){ if(stopParam){ dirY = !dirY; currentY = currentY - 3; for(int j=0;j<3;j++){ pwmY = 1; wait_us(350); pwmY = 0; wait_us(350); } break; }else{ if(speed == 0){ while(speed == 0){ speed = potenciometro_velocidade.read()*speedLimit; wait_ms(100); } } delay = (1000000/speed)/2; pwmY = 1; wait_us(delay); pwmY = 0; wait_us(delay); if(direction == 0){ currentY++; }else{ currentY--; } if(i < accelTime){ speed = speed + acceleration; }else if(i > (pulse - accelTime)){ speed = speed - acceleration; }else{ speed = potenciometro_velocidade.read()*speedLimit; accelTime = speed/acceleration; } } } }else{ dirZ = direction; int accelTime; int delay; int speed; if(accelParam == 0){ speed = potenciometro_velocidade.read()*speedLimit; accelTime = 0; }else{ speed = 1; accelTime = (potenciometro_velocidade.read()*speedLimit)/acceleration; } for(int i=0;i<pulse;i++){ if(stopParam){ dirZ = !dirZ; currentZ = currentZ - 3; for(int j=0;j<3;j++){ pwmZ = 1; wait_us(350); pwmZ = 0; wait_us(350); } break; }else{ if(speed == 0){ while(speed == 0){ speed = potenciometro_velocidade.read()*speedLimit; wait_ms(100); } } delay = (1000000/speed)/2; pwmZ = 1; wait_us(delay); pwmZ = 0; wait_us(delay); if(direction == 0){ currentZ++; }else{ currentZ--; } if(i < accelTime){ speed = speed + acceleration; }else if(i > (pulse - accelTime)){ speed = speed - acceleration; }else{ speed = potenciometro_velocidade.read()*speedLimit; accelTime = speed/acceleration; } } } } } //Move 2 motores void move2(int directionX, int directionY){ dirX = directionX; dirY = directionY; int delay; int speed = potenciometro_velocidade.read()*speedLimit; for(int i=0;i<40;i++){ if(stopParam == 1){ if(currentX > xMax){ dirX = !dirX; currentX = currentX - 3; for(int j=0;j<3;j++){ pwmX = 1; wait_us(delay); pwmX = 0; wait_us(delay); } }else if(currentY > yMax){ dirY = !dirY; currentY = currentY - 3; for(int j=0;j<3;j++){ pwmY = 1; wait_us(delay); pwmY = 0; wait_us(delay); } }else if(currentX > xMax && currentY > yMax){ dirX = !dirX; dirY = !dirY; currentX = currentX - 3; currentY = currentY - 3; for(int j=0;j<3;j++){ pwmX = 1; pwmY = 1; wait_us(delay); pwmX = 0; pwmY = 0; wait_us(delay); } } break; }else{ if(speed == 0){ while(speed == 0){ speed = potenciometro_velocidade.read()*speedLimit; wait_ms(100); } } delay = (1000000/speed)/2; pwmX = 1; pwmY = 1; wait_us(delay); pwmX = 0; pwmY = 0; wait_us(delay); if(directionX == 0){ currentX++; }else{ currentX--; } if(directionY == 0){ currentY++; }else{ currentY--; } speed = potenciometro_velocidade.read()*speedLimit; } } } //Define a Velocidade void set_speed(int newSpeed){ speedLimit = newSpeed; } //Define a Aceleração void set_acceleration(int newAccel){ acceleration = newAccel; } //Define as posições atuais como 0 void set_home(int motor){ if(motor == 1){ currentX = 0; }else if(motor == 2){ currentY = 0; }else{ currentZ = 0; } } //Move os motores até a posição de home void go_home(){ move(1,1,currentX); move(2,1,currentY); move(3,1,currentZ); } //Para o motor void hard_stop(){ stopParam = 1; } //Função de teste dos motores void setup_motor(){ motorEnable = 0; dirX = 1; dirY = 1; dirZ = 1; while(1){ move2(1,1); } } //FUNÇÕES DA MÁQUINA //Cria uma coordenada a partir do Input do usuário através do Teclado Matricial void setCoords(char eixo){ lcd.cls(); lcd.locate(1,2); if(eixo == 'x'){ user_inputX[0] = '+'; for(i=1;i<8;i++){ if(i != 4){ user_inputX[i] = '0'; wait_ms(20); } } lcd.printf("Valor %c=%s ",eixo,user_inputX); }else if(eixo == 'y'){ user_inputY[0] = '+'; for(i=1;i<8;i++){ if(i != 4){ user_inputY[i] = '0'; wait_ms(20); } } lcd.printf("Valor %c=%s ",eixo,user_inputY); }else{ user_inputZ[0] = '+'; for(i=1;i<8;i++){ if(i != 4){ user_inputZ[i] = '0'; wait_ms(20); } } lcd.printf("Valor %c=%s ",eixo,user_inputZ); } tempInt = 1; lcd.locate(1,0); lcd.printf("Pressione X para + e Y para -"); lcd.locate(10,3); lcd.printf("^"); while(1){ if(tempChar == 'u'){ lcd.locate(9+tempInt,3); lcd.printf(" "); if(tempInt == 3){ tempInt++; } if(tempInt < 7){ tempInt++; } lcd.locate(9+tempInt,3); lcd.printf("^"); }else if(tempChar == 'd'){ lcd.locate(9+tempInt,3); lcd.printf(" "); if(tempInt == 5){ tempInt--; } if(tempInt > 1){ tempInt--; } lcd.locate(9+tempInt,3); lcd.printf("^"); }else if(tempChar == 'x'){ if(eixo == 'x'){ user_inputX[0] = '+'; }else if(eixo == 'y'){ user_inputY[0] = '+'; }else{ user_inputZ[0] = '+'; } lcd.locate(9,2); lcd.printf("+"); }else if(tempChar == 'y'){ if(eixo == 'x'){ user_inputX[0] = '-'; }else if(eixo == 'y'){ user_inputY[0] = '-'; }else{ user_inputZ[0] = '-'; } lcd.locate(9,2); lcd.printf("-"); }else if(tempChar == 'z' || tempChar == ' '){ wait_ms(10); }else if(tempChar == 'e'){ while(tempChar == 'e'){ wait_ms(10); } lcd.cls(); lcd.locate(1,0); if(eixo == 'x'){ lcd.printf("Confirma o valor:\n %c=%s\n Confirmar\n Voltar",eixo,user_inputX); }else if(eixo == 'y'){ lcd.printf("Confirma o valor:\n %c=%s\n Confirmar\n Voltar",eixo,user_inputY); }else{ lcd.printf("Confirma o valor:\n %c=%s\n Confirmar\n Voltar",eixo,user_inputZ); } int c = 2; int min = 2; int max = 3; int row = min; int column = c; lcd.locate(column,row); lcd.printf("-"); while(tempChar != 'e'){ if(tempChar == 'd'){ lcd.locate(column,row); lcd.printf(" "); if(row == max){ row = min; }else{ row++; } lcd.locate(column,row); lcd.printf("-"); while(tempChar == 'd'){ wait_ms(10); } }else if(tempChar == 'u'){ lcd.locate(column,row); lcd.printf(" "); if(row == min){ row = max; }else{ row--; } lcd.locate(column,row); lcd.printf("-"); while(tempChar == 'u'){ wait_ms(10); } } wait_ms(10); } while(tempChar == 'e'){ wait_ms(10); } if(row == 2){ break; }else{ lcd.cls(); lcd.locate(1,0); lcd.printf("Pressione X para + e Y para -"); lcd.locate(1,2); if(eixo == 'x'){ lcd.printf("Valor %c=%s ",eixo,user_inputX); }else if(eixo == 'y'){ lcd.printf("Valor %c=%s ",eixo,user_inputY); }else{ lcd.printf("Valor %c=%s ",eixo,user_inputZ); } } }else{ if(eixo == 'x'){ user_inputX[tempInt] = tempChar; lcd.locate(9+tempInt,2); lcd.printf("%c",user_inputX[tempInt]); }else if(eixo == 'y'){ user_inputY[tempInt] = tempChar; lcd.locate(9+tempInt,2); lcd.printf("%c",user_inputY[tempInt]); }else{ user_inputZ[tempInt] = tempChar; lcd.locate(9+tempInt,2); lcd.printf("%c",user_inputZ[tempInt]); } lcd.locate(9+tempInt,3); lcd.printf(" "); if(tempInt == 3){ tempInt++; } if(tempInt < 7){ tempInt++; } lcd.locate(9+tempInt,3); lcd.printf("^"); } while(tempChar != ' '){ wait_ms(10); } wait_ms(100); } } //Move os motores para uma posição desejada void PosEixo(int xnew, int ynew, int znew){ if (xnew >= 0 && xnew <= xMax){ if (ynew >= 0 && ynew <= yMax){ if (znew >= 0 && znew <= zMax){ lcd.cls(); lcd.locate(3,1); lcd.printf("processando..."); set_speed(1500); while (abs(currentX-xnew) > 0){ tempInt = xnew-currentX; if(tempInt > 0){ move(1,0,tempInt); }else{ tempInt = abs(tempInt); move(1,1,tempInt); } } while (abs(currentY-ynew) > 0){ tempInt = ynew-currentY; if(tempInt > 0){ move(2,0,tempInt); }else{ tempInt = abs(tempInt); move(2,1,tempInt); } } while (abs(currentZ-znew) > 0){ tempInt = znew-currentZ; if(tempInt > 0){ move(3,0,tempInt); }else{ tempInt = abs(tempInt); move(3,1,tempInt); } } }else{ lcd.cls(); lcd.locate(2,1); lcd.printf("Coordenadas fora\n dos limites"); wait(2); } }else{ lcd.cls(); lcd.locate(2,1); lcd.printf("Coordenadas fora\n dos limites"); wait(2); } }else{ lcd.cls(); lcd.locate(2,1); lcd.printf("Coordenadas fora\n dos limites"); wait(2); } Teclado.attach(&checkTeclado,0.1); } //Função JOG - Move os motores a partir do joystick void Jog(){ accelParam = 0; lcd.cls(); lcd.locate(5,1); lcd.printf("Funcao Jog\n iniciada"); wait(2); lcd.cls(); lcd.locate(3,0); getPosmm(); lcd.printf("Eixo Z inativo\n X=%5.2fmm \n Y=%5.2fmm \n Z=%5.2fmm ",currentXmm,currentYmm,currentZmm); set_speed(1000); wait(1); while(1){ if(stopParam == 1){ stopParam = 0; if(currentX < 0){ while(stopParam == 0){ pwmX = 1; wait_ms(5); pwmX = 0; wait_ms(5); currentX++; } stopParam = 0; move(1,0,abs(currentX)); }else if(currentY < 0){ while(stopParam == 0){ pwmY = 1; wait_ms(5); pwmY = 0; wait_ms(5); currentY++; } stopParam = 0; move(2,0,abs(currentY)); }else if(currentX > xMax){ while(stopParam == 0){ pwmX = 1; wait_ms(5); pwmX = 0; wait_ms(5); currentX--; } stopParam = 0; tempInt = currentX-xMax; move(1,1,tempInt); }else if(currentY > yMax){ while(stopParam == 0){ pwmY = 1; wait_ms(5); pwmY = 0; wait_ms(5); currentY--; } stopParam = 0; tempInt = currentY-yMax; move(2,1,tempInt); } lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); while((vertical < 0.3 || vertical > 0.7) || (horizontal < 0.3 || horizontal > 0.7)){ vertical = yaxis.read(); horizontal = xaxis.read(); wait_ms(200); } }else{ if (vertical>0.9){ if (horizontal>0.9){ move2(1,1); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } else if (horizontal<0.1){ move2(0,1); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } else{ move(2,1,40); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } } else if (vertical<0.1){ if (horizontal>0.9){ move2(1,0); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } else if (horizontal<0.1){ move2(0,0); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } else{ move(2,0,40); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } } else{ if (horizontal>0.9){ move(1,1,40); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } else if (horizontal<0.1){ move(1,0,40); lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } else{ lcd.locate(5,1); getPosmm(); lcd.printf("X=%5.2fmm \n Y=%5.2fmm ",currentXmm,currentYmm); } } } if(tempChar == 'z'){ while(tempChar == 'z'){ wait_ms(10); } lcd.locate(3,0); lcd.printf("Eixo Z ativo "); while(tempChar != 'z' && tempChar != 'e'){ if(stopParam == 1){ stopParam = 0; if(currentZ < 0){ while(stopParam == 0){ pwmZ = 1; wait_ms(5); pwmZ = 0; wait_ms(5); currentZ++; } stopParam = 0; move(3,0,abs(currentZ)); }else if(currentZ > zMax){ while(stopParam == 0){ pwmZ = 1; wait_ms(5); pwmZ = 0; wait_ms(5); currentZ--; } stopParam = 0; tempInt = currentZ-zMax; move(3,1,tempInt); } lcd.locate(5,3); getPosmm(); lcd.printf("Z=%5.2fmm",currentZmm); while(horizontal < 0.3 || horizontal > 0.7){ horizontal = xaxis.read(); wait_ms(200); } }else{ if (vertical>0.9){ move(3,1,40); lcd.locate(5,3); getPosmm(); lcd.printf("Z=%5.2fmm ",currentZmm); } else if (vertical<0.1){ move(3,0,40); lcd.locate(5,3); getPosmm(); lcd.printf("Z=%5.2fmm ",currentZmm); } else{ lcd.locate(5,3); getPosmm(); lcd.printf("Z=%5.2fmm ",currentZmm); } if (horizontal>0.9){ if(grip_state==1){ wait_ms(50); } else{ enable_grip=1; close_grip_var=0; open_grip_var=1; lcd.locate(19,3); lcd.printf(" "); wait_ms(800); lcd.locate(19,3); lcd.printf("A"); grip_state=1; } } else if (horizontal<0.1){ if(grip_state==0){ wait_ms(50); } else{ enable_grip=1; open_grip_var=0; close_grip_var=1; lcd.locate(19,3); lcd.printf(" "); wait_ms(800); lcd.locate(19,3); lcd.printf("F"); grip_state=0; } } else{ enable_grip=0; } } vertical = yaxis.read(); horizontal = xaxis.read(); } while(tempChar == 'z'){ wait_ms(10); } lcd.locate(3,0); lcd.printf("Eixo Z inativo"); } if(tempChar == 'e'){ while(tempChar == 'e'){ wait_ms(10); } break; } vertical = yaxis.read(); horizontal = xaxis.read(); } accelParam = 1; } //Referencia os eixos com base no acionamento dos sensores de fim de curso void ReferEixo(char eixo){ Teclado.detach(); accelParam = 0; stopParam = 0; tempInt = (int)(3.0*mm_pulse); lcd.cls(); lcd.locate(3,1); lcd.printf("processando..."); if (eixo == 'x'){ set_speed(1500); while(stopParam == 0){ move(1,1,80); } stopParam = 0; set_speed(100); while(stopParam == 0){ move(1,0,80); } stopParam = 0; set_speed(750); move(1,0,tempInt); set_home(1); wait_ms(100); set_speed(1500); while(stopParam == 0){ move(1,0,80); } stopParam = 0; set_speed(100); while(stopParam == 0){ move(1,1,80); } stopParam = 0; set_speed(750); move(1,1,tempInt); xMax = currentX; } else if(eixo =='g'){ enable_grip=1; open_grip_var=1; close_grip_var=0; wait(1); enable_grip=0; close_grip_var=0; grip_state=0; }else if (eixo == 'y'){ set_speed(1500); while(stopParam == 0){ move(2,1,80); } stopParam = 0; set_speed(100); while(stopParam == 0){ move(2,0,80); } stopParam = 0; set_speed(750); move(2,0,tempInt); set_home(2); wait_ms(100); set_speed(1500); while(stopParam == 0){ move(2,0,80); } stopParam = 0; set_speed(100); while(stopParam == 0){ move(2,1,80); } stopParam = 0; set_speed(750); move(2,1,tempInt); yMax = currentY; }else{ set_speed(1500); while(stopParam == 0){ move(3,1,80); } stopParam = 0; set_speed(100); while(stopParam == 0){ move(3,0,80); } stopParam = 0; set_speed(750); move(3,0,tempInt); set_home(3); wait_ms(100); set_speed(1500); while(stopParam == 0){ move(3,0,80); } stopParam = 0; set_speed(100); while(stopParam == 0){ move(3,1,80); } stopParam = 0; set_speed(750); move(3,1,tempInt); zMax = currentZ; } Teclado.attach(&checkTeclado,0.1); accelParam = 1; } //Cria uma classe de menu para o LCD class Menu{ public: int row; int column; int func; void starting(){ lcd.cls(); lcd.locate(5,1); lcd.printf("Pet Finder\n Maquina separadora"); wait(3); func = 2; } void referenced(){ lcd.cls(); lcd.locate(0,1); lcd.printf("Maquina referenciada"); wait(3); func = 6; } void notReferenced(int mode = 1){ if(mode == 1){ lcd.cls(); lcd.locate(4,1); lcd.printf("Maquina nao\n referenciada"); wait(2); lcd.cls(); lcd.locate(1,0); lcd.printf("Referenciar agora?"); lcd.locate(9,1); lcd.printf("Sim"); lcd.locate(9,2); lcd.printf("Nao"); moveCursor(8,1,2); if(row==1){ func = 5; }else{ func = 6; } }else if(mode == 2){ lcd.cls(); lcd.locate(4,1); lcd.printf("Referencie a\n maquina para\n usar essa funcao"); wait(2); func = 6; }else if(mode == 3){ lcd.cls(); lcd.locate(4,1); lcd.printf("Maquina nao\n referenciada"); wait(2); func = 6; } } void referencing(){ lcd.cls(); lcd.locate(1,0); lcd.printf("Selecione um eixo:"); if (xMax==-1){ lcd.locate(3,1); lcd.printf("Eixo X"); } else{ lcd.locate(3,1); lcd.printf("Eixo X*"); } if (yMax==-1){ lcd.locate(3,2); lcd.printf("Eixo Y"); } else{ lcd.locate(3,2); lcd.printf("Eixo Y*"); } if (zMax==-1){ lcd.locate(3,3); lcd.printf("Eixo Z"); } else{ lcd.locate(3,3); lcd.printf("Eixo Z*"); } if (grip_state==-1){ lcd.locate(13,1); lcd.printf("Garra"); } else{ lcd.locate(13,1); lcd.printf("Garra*"); } lcd.locate(13,2); lcd.printf("Back"); wait_ms(10); moveCursor(2,1,3,12,1,2); if(column == 2){ if(row == 1){ ReferEixo('x'); }else if(row == 2){ ReferEixo('y'); }else{ ReferEixo('z'); } func = 5; }else{ if(row == 1){ ReferEixo('g'); }else{ if(xMax == -1 || yMax == -1 || zMax == -1 || grip_state == -1){ func = 4; }else{ func = 1; } } } } void moveCursor(int c, int min, int max, int c2 = -1, int min2 = -1, int max2=-1){ row = min; column = c; if(c2 == -1 || min2 == -1 || max2 == -1){ c2 = c; min2 = min; max2 = max; } lcd.locate(column,row); lcd.printf("-"); while(tempChar != 'e'){ if(tempChar == 'd'){ lcd.locate(column,row); lcd.printf(" "); if(row == max && column == c){ row = min2; column = c2; }else if(row == max2 && column == c2){ row = min; column = c; }else{ row++; } lcd.locate(column,row); lcd.printf("-"); while(tempChar == 'd'){ wait_ms(10); } }else if(tempChar == 'u'){ lcd.locate(column,row); lcd.printf(" "); if(row == min && column == c){ row = max2; column = c2; }else if(row == min2 && column == c2){ row = max; column = c; }else{ row--; } lcd.locate(column,row); lcd.printf("-"); while(tempChar == 'u'){ wait_ms(10); } } wait_ms(10); } while(tempChar == 'e'){ wait_ms(10); } } void rootMenu(){ lcd.cls(); lcd.locate(1,0); lcd.printf("Referenciamento\n Controle manual\n Posicoes\n Executar"); moveCursor(0,0,3); if(row == 0){ func = 5; }else if(row == 1){ func = 7; }else if(row == 2){ func = 8; }else{ func = 12; } } void manualControl(){ if(xMax == -1 || yMax == -1 || zMax == -1 || grip_state == -1){ func = 3; }else{ lcd.cls(); lcd.locate(1,0); lcd.printf("Voltar\n JOG\n Inserir coordenadas"); moveCursor(0,0,2); if(row == 0){ func = 6; }else if(row == 1){ Jog(); lcd.cls(); lcd.locate(1,0); getPosmm(); lcd.printf("Salvar posicao?\nX:%5.2f\nY:%5.2f\nZ:%5.2f",currentXmm,currentYmm,currentZmm); lcd.locate(12,2); lcd.printf("Sim"); lcd.locate(12,3); lcd.printf("Nao"); moveCursor(11,2,3); if(row == 2){ inputX = currentX; inputY = currentY; inputZ = currentZ; func = 10; }else{ func = 7; } }else{ setCoords('x'); setCoords('y'); setCoords('z'); ConvertmmPulse(atof(user_inputX),atof(user_inputY),atof(user_inputZ)); inputX = currentX+inputX; inputY = currentY+inputY; inputZ = currentZ+inputZ; lcd.cls(); lcd.locate(1,1); lcd.printf("Salvar\n Ir para posicao"); moveCursor(0,1,2); if(row == 1){ if(inputX >= 0 && inputX <= xMax){ if(inputY >= 0 && inputY <= yMax){ if(inputZ >= 0 && inputZ <= zMax){ func = 10; }else{ lcd.cls(); lcd.locate(2,1); lcd.printf("Coordenadas fora\n dos limites"); wait(2); func = 7; } }else{ lcd.cls(); lcd.locate(2,1); lcd.printf("Coordenadas fora\n dos limites"); wait(2); func = 7; } }else{ lcd.cls(); lcd.locate(2,1); lcd.printf("Coordenadas fora\n dos limites"); wait(2); func = 7; } }else{ PosEixo(inputX,inputY,inputZ); lcd.cls(); lcd.locate(1,1); lcd.printf("Salvar\n Finalizar"); moveCursor(0,1,2); if(row == 1){ func = 10; }else{ func = 7; } } } } } void positions(){ lcd.cls(); lcd.locate(1,0); lcd.printf("Voltar\n Ver posicoes\n Deletar posicoes"); moveCursor(0,0,2); if(row == 0){ func = 6; }else if(row == 1){ func = 9; }else{ func = 11; } } void selectPos(int mode = 1){ lcd.cls(); lcd.locate(1,0); lcd.printf("Voltar"); lcd.locate(9,0); lcd.printf("Pegar\n Soltar 1\n Soltar 2\n Soltar 3"); moveCursor(0,0,0,8,0,3); if(column == 0){ func = 8; }else{ if(row == 0){ if(mode == 1){ ConvertPulsemm(pickCoords[0],pickCoords[1],pickCoords[2]); lcd.cls(); lcd.locate(1,0); lcd.printf("Posicao de pegar\n Eixo X: %5.2f\n Eixo Y: %5.2f\n Eixo Z: %5.2f",inputXmm,inputYmm,inputZmm); while(tempChar != 'e'){ wait_ms(10); } func = 9; }else if(mode == 2){ pickCoords[0] = inputX; pickCoords[1] = inputY; pickCoords[2] = inputZ; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n pegar salva"); wait(2); func = 7; }else{ lcd.cls(); lcd.locate(1,0); lcd.printf("Esta certo disso?\n Sim\n Nao"); moveCursor(2,1,2); if(row == 1){ pickCoords[0] = 0; pickCoords[1] = 0; pickCoords[2] = 0; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n pegar deletada"); wait(2); func = 9; }else{ func = 11; } } }else if(row == 1){ if(mode == 1){ ConvertPulsemm(placeCoords1[0],placeCoords1[1],placeCoords1[2]); lcd.cls(); lcd.locate(0,0); lcd.printf("Posicao de soltar 1\n Eixo X: %5.2f\n Eixo Y: %5.2f\n Eixo Z: %5.2f",inputXmm,inputYmm,inputZmm); while(tempChar != 'e'){ wait_ms(10); } func = 9; }else if(mode == 2){ placeCoords1[0] = inputX; placeCoords1[1] = inputY; placeCoords1[2] = inputZ; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n soltar1 salva"); wait(2); func = 7; }else{ lcd.cls(); lcd.locate(1,0); lcd.printf("Esta certo disso?\n Sim\n Nao"); moveCursor(2,1,2); if(row == 1){ placeCoords1[0] = 0; placeCoords1[1] = 0; placeCoords1[2] = 0; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n soltar 1 deletada"); wait(2); func = 9; }else{ func = 11; } } }else if(row == 2){ if(mode == 1){ ConvertPulsemm(placeCoords2[0],placeCoords2[1],placeCoords2[2]); lcd.cls(); lcd.locate(0,0); lcd.printf("Posicao de soltar 2\n Eixo X: %5.2f\n Eixo Y: %5.2f\n Eixo Z: %5.2f",inputXmm,inputYmm,inputZmm); while(tempChar != 'e'){ wait_ms(10); } func = 9; }else if(mode == 2){ placeCoords2[0] = inputX; placeCoords2[1] = inputY; placeCoords2[2] = inputZ; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n soltar2 salva"); wait(2); func = 7; }else{ lcd.cls(); lcd.locate(1,0); lcd.printf("Esta certo disso?\n Sim\n Nao"); moveCursor(2,1,2); if(row == 1){ placeCoords2[0] = 0; placeCoords2[1] = 0; placeCoords2[2] = 0; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n soltar2 deletada"); wait(2); func = 9; }else{ func = 11; } } }else{ if(mode == 1){ ConvertPulsemm(placeCoords3[0],placeCoords3[1],placeCoords3[2]); lcd.cls(); lcd.locate(0,0); lcd.printf("Posicao de soltar 3\n Eixo X: %5.2f\n Eixo Y: %5.2f\n Eixo Z: %5.2f",inputXmm,inputYmm,inputZmm); while(tempChar != 'e'){ wait_ms(10); } func = 9; }else if(mode == 2){ placeCoords3[0] = inputX; placeCoords3[1] = inputY; placeCoords3[2] = inputZ; lcd.cls(); lcd.locate(5,1); lcd.printf(" Posicao de\n soltar3 salva"); wait(2); func = 7; }else{ lcd.cls(); lcd.locate(1,0); lcd.printf(" Esta certo disso?\n Sim\n Nao"); moveCursor(2,1,2); if(row == 1){ placeCoords3[0] = 0; placeCoords3[1] = 0; placeCoords3[2] = 0; lcd.cls(); lcd.locate(5,1); lcd.printf("Posicao de\n soltar3 deletada"); wait(2); func = 9; }else{ func = 11; } } } } } void run(){ type_1=0; type_2=0; type_3=0; if(xMax == -1 || yMax == -1 || zMax == -1 || grip_state == -1){ func = 3; }else{ open_grip(); int break_all = 0; while(1){ PosEixo(currentX,currentY,400); PosEixo(pickCoords[0],pickCoords[1],400); PosEixo(currentX,currentY,pickCoords[2]); /*lcd.cls(); lcd.locate(3,1); lcd.printf("aguardando..."); while(1){ esperar uma garrafa ser posicionada rotina_rgb(); cs=!capacitive.read(); if (rgb=='r' || rgb=='a'){ for (int tempo=5;tempo>-1;i--){ lcd.cls(); lcd.locate(0,0); lcd.printf(" Frasco Detectado\n Executando em\n %ds",tempo); } rotina_rgb(); if(rgb=='r' || rgb=='a'){ break; } else{ lcd.cls(); lcd.locate(0,1); lcd.printf(" Frasco\n nao\n detectado"); } } else{ wait_ms(50); } if(tempChar == 'e'){ lcd.cls(); while(tempChar == 'e'){ wait_ms(10); } lcd.cls(); lcd.locate(0,0); lcd.printf(" Frascos Separados:\n Rosa e Cheio:%d \n Azul e Cheio:%d \n Vazios:%d ",type_1,type_2,type_3); wait(3); break_all = 1; break; } }*/ if(break_all == 1){ break; } if(rgb == ' '){ cs = 1; rgb = 'r'; }else if(rgb == 'r'){ cs = 1; rgb = 'a'; }else{ cs = 0; rgb = ' '; } //close_grip(); if(cs==1){ if (rgb=='r'){ selection = 1; } else if(rgb=='a'){ selection = 2; } else{ selection = 0; } } else{ selection = 3; } close_grip(); PosEixo(currentX,currentY,400); if(selection == 0){ lcd.cls(); lcd.locate(2,1); lcd.printf("Erro na medicao"); PosEixo(currentX,currentY,pickCoords[2]); }else if(selection == 1){ lcd.cls(); lcd.locate(0,0); lcd.printf("Cor da Tampa:\n Rosa\n Estado do Frasco:\n Cheio"); PosEixo(placeCoords1[0],placeCoords1[1],400); PosEixo(currentX,currentY,placeCoords1[2]); type_1++; }else if(selection == 2){ lcd.cls(); lcd.locate(0,0); lcd.printf("Cor da Tampa:\n Azul\n Estado do Frasco:\n Cheio"); PosEixo(placeCoords2[0],placeCoords2[1],400); PosEixo(currentX,currentY,placeCoords2[2]); type_2++; }else{ lcd.cls(); lcd.locate(3,0); lcd.printf("Frasco Vazio"); PosEixo(placeCoords3[0],placeCoords3[1],400); PosEixo(currentX,currentY,placeCoords3[2]); type_3++; } open_grip(); lcd.cls(); lcd.locate(0,0); lcd.printf(" Frasco Separado\n com Sucesso"); wait(2); break; } func = 6; } } Menu(); }; Menu::Menu(){ func = 0; } Menu menu; int main() { FDC.rise(&hard_stop); FDC.fall(&hard_stop); motorEnable = 0; Teclado.attach(&checkTeclado,0.1); set_home(1); //Tirar no codigo set_home(2); //Tirar no codigo set_home(3); //Tirar no codigo ConvertmmPulse(409.7,329.8,44.4); xMax=inputX; //Tirar no codigo; yMax=inputY; //Tirar no codigo zMax=inputZ; //Tirar no codigo grip_state=0; while(1){ switch(menu.func){ case 0: menu.starting(); break; case 1: menu.referenced(); break; case 2: menu.notReferenced(); break; case 3: menu.notReferenced(2); break; case 4: menu.notReferenced(3); break; case 5: menu.referencing(); break; case 6: menu.rootMenu(); break; case 7: menu.manualControl(); break; case 8: menu.positions(); break; case 9: menu.selectPos(); break; case 10: menu.selectPos(2); break; case 11: menu.selectPos(3); break; case 12: menu.run(); break; } wait_ms(200); } }