File content as of revision 0:924ec4a5d21b:

/* Includes ------------------------------------------------------------------*/

/* mbed specific header files. */
#include "mbed.h"
//#include "TCS3200.h"

#include "TextLCD.h"


TextLCD lcd(PC_13, PB_7, PA_14, PB_12, PB_2, PB_11, TextLCD::LCD20x4); // rs, e, d4-d7

    //lcd.setBacklight(TextLCD::LightOn);
    //lcd.printf("Billy gordo");

//#include "TCS3200.h"
//TCS3200 color(PC_4, PB_13, PB_14, PB_15, PA_15);  //Create a TCS3200 object 
//            S0    S1    S2     S3     OUT


/*------------------------- Implementação LCD --------------------------------*/

//#include "TextLCD.h"

// Host PC Communication channels
//Serial pc(USBTX, USBRX); // tx, rx

// I2C Communication
//I2C i2c_lcd(D14,D15); // SDA, SCL

//TextLCD_I2C lcd(&i2c_lcd, 0x7E, TextLCD::LCD20x4);


/* Definitions ---------------------------------------------------------------*/



/* Variables -----------------------------------------------------------------*/
//InterruptIn Botao_X(D11);
//InterruptIn Botao_Y(D12);
//InterruptIn Botao_Z(D13);
//InterruptIn Botao_Ref(PC_12);
//InterruptIn Botao_Ref_Go(PC_8);
//InterruptIn Botao_positivo(PC_2);
InterruptIn FDCX1(PC_9);
InterruptIn FDCX2(PB_9);\
InterruptIn FDCY1(PB_9);
InterruptIn FDCY2(PB_6);
InterruptIn FDCZ1(PA_7);
InterruptIn FDCZ2(PC_7);
InterruptIn botaox1(PC_3);
InterruptIn botaoy1(PC_5);
InterruptIn botaoz1(PC_10);
InterruptIn botaox2(PA_13);
InterruptIn botaoy2(PC_2);
InterruptIn botaoz2(PC_12);
InterruptIn STOP(PA_11);
InterruptIn HANDL(PC_8);
//InterruptIn FDCZ1(PC_2);
//InterruptIn FDCZ2(PC_2);

//ENTRADAS MOTOR
DigitalOut Eixo_X_Pulso(D2);
DigitalOut Eixo_Y_Pulso(D3);
DigitalOut Eixo_Z_Pulso(D4);
DigitalOut Eixo_X_Dir(D5);
DigitalOut Eixo_Y_Dir(D6);
DigitalOut Eixo_Z_Dir(D7);
DigitalOut Enable(D8);
DigitalOut Valv(PB_0);
DigitalOut LEDSTART(PA_4);
DigitalOut LEDSTOP(PA_1);
DigitalOut LEDHANDLE(PA_0);

//ENTRADAS MOTOR

DigitalIn Capac(PC_1);
DigitalIn START(PC_0);


//DigitalIn Botao_Y_positivo(A0);
//DigitalIn Botao_Y_negativo(A1);

//DigitalIn Botao_Z_positivo(A0);
//DigitalIn Botao_Z_negativo(A1);

//InterruptIn Botao_FIM_X_MAX(A4);
//InterruptIn Botao_FIM_X_MIN(A5);

//DigitalIn Botao_FIM_X_MAX(A3);
//DigitalIn Botao_FIM_X_MIN(A4);

//DigitalIn Botao_FIM_X_MAX(A3);
//DigitalIn Botao_FIM_X_MIN(A4);

int pos_gar_x = 0;
int pos_gar_y = 0;
int pos_gar_z = 0;
int solta_x1 = 0;
int solta_x2 = 0;
int solta_x3 = 0;
int solta_y1 = 0;
int solta_y2 = 0;
int solta_y3 = 0;
int solta_z1 = 0;
int solta_z2 = 0;
int solta_z3 = 0;
int pos_x1 = 0;
int pos_x2 = 0;
int pos_x3 = 0;
int pos_y1 = 0;
int pos_y2 = 0;
int pos_y3 = 0;
int pos_z1 = 0;
int pos_z2= 0;
int pos_z3 = 0;
int zera = 0;
int HANDLE = 0;
int STARTCODE = 0;
int posmidx = 0;
int posmidy = 0;
int posmidz = 0;

//var do motor
bool dir = false;
int cont_pulses = 0;
int cur_pos_x = 0;
int cur_pos_y = 0;
int cur_pos_z = 0;
float velocidade = 0.00032;//de 0.01 a 0.0004
float velocidade2 = 0.0005;
bool auto_rot = false;
//var do motor



/*------------------------ Funções --------------------------*/

void ligabobina(){
    Valv = 1;
    }

void desligabobina(){
    Valv = 0;
    }
/*    
int rgb() { //--------------------------------------PERFEITO
    int variavel;
    long red, green, blue, clear;
    color.SetMode(TCS3200::SCALE_20);
    while(x<100){
        red = color.ReadRed();
        green = color.ReadGreen();
        blue = color.ReadBlue();
        clear = color.ReadClear();
        //printf("RED: %10d     GREEN: %10d     BLUE: %10d     CLEAR: %10d    \r\n ", red, green, blue, clear);
        if((red<green)&&(red<blue)){
            //printf("Color: Red \r\n");
            variavel=1
        }
        if((green<red)&&(green<blue)){
            //printf("Color: Green \r\n");
        }
        if((blue<red)&&(blue<green)){
            //printf("Color: Blue \r\n");
        }
        variavel = 0;
        wait(0.1);
        x++;
        }
        return variavel;
}
*/
/*
int rgb(){
     long red, green, blue, clear;
     
     //Set the scaling factor to 100%
      color.SetMode(TCS3200::SCALE_100);        
    int bob=0;
    int cor=0;
      while(bob<10){
          //Read the HIGH pulse width in nS for each color.
          //The lower the value, the more of that color is detected 
          red = color.ReadRed();
          green = color.ReadGreen();
          blue = color.ReadBlue();
          clear = color.ReadClear();
 
          printf("RED: %10d     GREEN: %10d     BLUE: %10d     CLEAR: %10d     ", red, green, blue, clear);
             
          wait(0.1);
 
        if(red>50)
         {
             printf("VERMELHO. \r\n"); // Vermelho
             cor=1;
         }
        if(blue>50)
         {
             printf("AZUL. \r\n"); // Azul
             cor=0;
         }
                
        else
         {
             printf("VERDE. \r\n"); // Verde
          }
          bob++;
          }

    return cor;
    }    
  */  
  int rgb(){
      return;
      }
      
int capacitivo(void){
    int y=0;
    int C=0;
    while (y<10){
    int C= Capac;
    y++;
    }
    return C;
    }

void direcao(int motor,int posf){
    dir = true ;
    switch(motor){
        case 1:
        if (posf<cur_pos_x){
        while(dir == true && cur_pos_x !=posf){
        Eixo_X_Dir = 0;
        Eixo_X_Pulso = 1;
        wait(velocidade);
        Eixo_X_Pulso = 0;
                wait(velocidade);
        cur_pos_x -= 1;
        //dir == false;
        }}
        
        if (posf>cur_pos_x){
        while(dir == true && cur_pos_x !=posf){
        Eixo_X_Dir = 1;
        Eixo_X_Pulso = 1;
        wait(velocidade);
        Eixo_X_Pulso = 0;
                wait(velocidade);
        cur_pos_x += 1;
        //dir == false;
        }}
        break;
            
        case 2:
        if (posf<cur_pos_y){
        while(dir==true && cur_pos_y !=posf){
        Eixo_Y_Dir = 0;
        Eixo_Y_Pulso = 1;
        wait(velocidade);
        Eixo_Y_Pulso = 0;
                wait(velocidade);
        cur_pos_y -= 1;
        //dir == false;
          }}
        
        if (posf>cur_pos_y){
        while(dir==true && cur_pos_y !=posf){
        Eixo_Y_Dir = 1;
        Eixo_Y_Pulso = 1;
        wait(velocidade);
        Eixo_Y_Pulso = 0;
                wait(velocidade);
        cur_pos_y += 1;
        //dir == false;
        }}
        
        break;
                            
        case 3:
        if (posf<cur_pos_z){
        while(dir == true && cur_pos_z !=posf){
        Eixo_Z_Dir = 0;
        Eixo_Z_Pulso = 1;
                wait(velocidade);
        Eixo_Z_Pulso = 0;
                wait(velocidade);
        cur_pos_z -= 1;
        //dir == false;
                }
        }
        if (posf>cur_pos_z){
        while(dir == true && cur_pos_z !=posf){
        Eixo_Z_Dir = 1;
        Eixo_Z_Pulso = 1;
        wait(velocidade);
        Eixo_Z_Pulso = 0;
                wait(velocidade);
        cur_pos_z += 1;
        //dir == false;
                }
        }
        break;  
    
    }       
    return;
    }  


void direcao_p(int motor){
    dir = true ;
    switch(motor){
        while(dir == true){
        case 1:
        Eixo_X_Dir = 1;
        Eixo_X_Pulso = 1;
        wait(velocidade);
        Eixo_X_Pulso = 0;
                wait(velocidade);
        cur_pos_x += 1;
        //dir == false;
        }
        break;
                
                
        case 2:
        while(dir==true){
        Eixo_Y_Dir = 1;
        Eixo_Y_Pulso = 1;
        wait(velocidade);
        Eixo_Y_Pulso = 0;
                wait(velocidade);
        cur_pos_y += 1;
        //dir == false;
                }
        break;
                
                
        case 3:
        while(dir == true){
        Eixo_Z_Dir = 1;
        Eixo_Z_Pulso = 1;
        wait(velocidade);
        Eixo_Z_Pulso = 0;
                wait(velocidade);
        cur_pos_z += 1;
        //dir == false;
                }
        break;
                
                
    
    }
    return;
    }

void FDCY(){
    cur_pos_y=0;
    direcao(2,1500);
    cur_pos_y=0;
}    

void motor_off(){
    dir = false;
    zera++;
    }

void direcao_n(int motor){
    dir = true ;
    switch(motor){
        while(dir == true){
        case 1:
        Eixo_X_Dir = 0;
        Eixo_X_Pulso = 1;
        wait(velocidade);
        Eixo_X_Pulso = 0;
                wait(velocidade);
        cur_pos_x -= 1;
        //dir == false;
        }
        break;
                
                
        case 2:
        while(dir==true){
        Eixo_Y_Dir = 0;
        Eixo_Y_Pulso = 1;
        wait(velocidade);
        Eixo_Y_Pulso = 0;
                wait(velocidade);
        cur_pos_y -= 1;
        //dir == false;
          }
        break;
                
                
        case 3:
        while(dir == true){
        Eixo_Z_Dir = 0;
        Eixo_Z_Pulso = 1;
                wait(velocidade);
        Eixo_Z_Pulso = 0;
                wait(velocidade);
        cur_pos_z -= 1;
        //dir == false;
                }
        break;
            
    }
    Eixo_X_Pulso = 0;
    Eixo_Y_Pulso = 0;
    Eixo_Z_Pulso = 0;        
    return;
    } 

void parax(){
     direcao_n(1);
     }
        
void paray(){
     printf("mexeu");
     direcao_n(2);
     
     }
       
void paraz(){
     direcao_p(3);
     }
                          
     
void zeramentox(){
    //if (zera==0){
    printf("ff");
    lcd.locate(0,0);
    lcd.printf("Zerando eixo x");
    direcao_n(1);
    }

void zeramentoy(){
    lcd.locate(0,0);
    lcd.printf("Zerando eixo y");
    //if (zera==1){
    direcao_p(2);
    }
    
void zeramentoz(){
   lcd.locate(0,0);
   lcd.printf("Zerando eixo z");
   // if (zera==2){
    direcao_n(3);
    }

void handd(){
    HANDLE=0;
    LEDHANDLE = 0;
    }
    
void ciclo_off(){
    dir = false;
    STARTCODE=0;
    dir = false;
    
    }
    
void ciclo(int xp, int yp, int zp){
    direcao(3,posmidz);
    STARTCODE = 0;
    LEDSTART = 1;
    STOP.rise(&ciclo_off);
    //direcao(1,posmidx);
   // direcao(2,posmidy);
   while (1){
    if (START ==1){
        STARTCODE=1;
        }
   while (STARTCODE == 1){
    LEDSTOP = 1;
    direcao(3,zp);
    ligabobina();
    direcao(3,posmidz);
    int ccor = rgb();
    int capa = capacitivo();
    lcd.printf("%d",ccor);
    wait(3);
    if (ccor==1 && capa == 1 ){
        direcao(1,solta_x1);
        direcao(2,solta_y1);
        direcao(3,zp+100);
        desligabobina();
        direcao(3,posmidz);
        direcao(1,xp);
        direcao(2,yp);

        }
    if (ccor == 2 && capa == 1){
        direcao(1,solta_x2);
        direcao(2,solta_y2);
        direcao(3,zp+100);
        desligabobina();
        direcao(3,posmidz);
        direcao(1,xp);
        direcao(2,yp);
        
        }
        
    if (capa == 0){
        direcao(1,solta_x3);
        direcao(2,solta_y3);
        direcao(3,zp+100);
        desligabobina();
        direcao(3,posmidz);
        direcao(1,xp);
        direcao(2,yp);
        
        }
    }
    //print start
    }
    }
    

void JOG(){
    lcd.locate(0,0);
    lcd.printf("JOG ativado");
    lcd.locate(0,1);
    lcd.printf("X = %f",cur_pos_x/400);
    lcd.locate(0,2);
    lcd.printf("Y = %f",cur_pos_y/400);
    lcd.locate(0,3);
    lcd.printf("Z = %f",cur_pos_z/400);
    HANDL.rise(&handd);
    HANDLE = 1;
    LEDHANDLE = 1;
    //botaox.rise(&parax);
    botaox1.fall(&motor_off);
    botaoy1.fall(&motor_off);
    botaoz1.fall(&motor_off);
    botaox2.fall(&motor_off);
    botaoy2.fall(&motor_off);
    botaoz2.fall(&motor_off);
    wait(2);
    while(1){
    while(HANDLE==1){
        lcd.cls();
        lcd.locate(0,0);
        lcd.printf("JOG ativado");
        lcd.locate(0,1);
        float bobaox=3*cur_pos_x/400;
        float bobaoz=3*cur_pos_z/400;
        float bobaoy=3*cur_pos_y/400;
        lcd.printf("X = %.2f mm",bobaox);
        lcd.locate(0,2);
        lcd.printf("Y = %.2f mm",bobaoy);
        lcd.locate(0,3);
        lcd.printf("Z = %.2f mm",bobaoz);
        if (botaox1==1){
            direcao_n(1);
            }
        if (botaoy1 == 1){
            direcao_n(2);
            }
        if (botaoz1 == 1){
            direcao_n(3);
            }
        if (botaox2==1){
            direcao_p(1);
            }
        if (botaoy2 == 1){
            direcao_p(2);
            }
        if (botaoz2 == 1){
            direcao_p(3);
            }
            wait(0.2);
        }
    
    if (HANDLE == 0){
        lcd.cls();
        lcd.printf("Pos PEGAR Salva!");
        pos_gar_x=cur_pos_x;
        pos_gar_y=cur_pos_y;
        pos_gar_z=cur_pos_z;
        wait(2);        
        ciclo(pos_gar_x,pos_gar_y,pos_gar_z);
        break;
        }
        printf("nada");
        }}
            

    
int main(){
        //lcd.setBacklight(TextLCD::LightOn);
        lcd.printf("Zeramento em 5");
        wait(1);
        lcd.cls();
        lcd.printf("Zeramento em 4");
        wait(1);
        lcd.cls();
        lcd.printf("Zeramento em 3");
        wait(1);
        lcd.cls();
        lcd.printf("Zeramento em 2");
        wait(1);
        lcd.cls();
        lcd.printf("Zeramento em 1");
        wait(1);
        lcd.cls();
        lcd.printf("Zerando");
        FDCY1.rise(&motor_off);
        FDCX1.rise(&motor_off);
        FDCZ1.rise(&motor_off);
        zeramentox();
        cur_pos_x=0;
        lcd.locate(0,1);
        lcd.printf("PosX Salva!");
        wait(1);
        zeramentoy();
        cur_pos_y=0;
        lcd.locate(0,2);
        lcd.printf("PosY Salva!");
        wait(1);
        zeramentoz();//ok
        lcd.locate(0,3);
        lcd.printf("PosZ Salva!");
        cur_pos_z=0;
        wait(1);
        JOG();
        printf("saiu");
        //ciclo(pos_gar_x,pos_gar_y,pos_gar_z);
                
        }