Gabriel Patrocinio
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);
}
}