Gabriel Patrocinio
Projeto Mecatrônico - Pick and Place
Revision 0:366afb0a6154, committed 2017-06-06
- Comitter:
- gabrielpp
- Date:
- Tue Jun 06 02:11:55 2017 +0000
- Commit message:
- C?digo final do Projeto Mecatr?nico
Changed in this revision
diff -r 000000000000 -r 366afb0a6154 TextLCD_HelloWorld2.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD_HelloWorld2.lib Tue Jun 06 02:11:55 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/ViniR/code/MenuLCD/#8ed644ed983b
diff -r 000000000000 -r 366afb0a6154 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jun 06 02:11:55 2017 +0000
@@ -0,0 +1,1745 @@
+#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);
+
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 366afb0a6154 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Jun 06 02:11:55 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/86740a56073b \ No newline at end of file