Projeto Mecatrônico
testando
Diff: main_PM.cpp
- Revision:
- 72:aa3902facc8c
- Parent:
- 71:c91c96bbed54
- Child:
- 74:53c345d557e8
--- a/main_PM.cpp Wed Jun 03 15:52:56 2020 +0000
+++ b/main_PM.cpp Wed Jun 03 18:56:16 2020 +0000
@@ -5,6 +5,7 @@
#include "classe.h"
#include <stdio.h>
#include <string>
+#define PI 3.14159265
//local para a declaracao de varioveis
float t_motor = 0.002; //delay entre o acionamento de uma bobina do motor e a outra
@@ -15,10 +16,12 @@
char tecla; //variável que guarda a tecla apertada no teclado
float coord_x = 0; //variável que guardará a posição linear do eixo x, ela é incrementada com a conta do deslocamento linear dado um deslocamento angular sempre que o eixo é movimentado depois do referencimaneto
float coord_y = 0; //variável que guardará a posição linear do eixo y, ela é incrementada com a conta do deslocamento linear dado um deslocamento angular sempre que o eixo é movimentado depois do referencimaneto
-float coord_z = 0; //variável que guardará a posição linear do eixo z, ela é incrementada com a conta do deslocamento linear dado um deslocamento angular sempre que o eixo é movimentado depois do referencimaneto
+float coord_z = 0;
+float normal_z = 100; //variável que guardará a posição linear do eixo z, ela é incrementada com a conta do deslocamento linear dado um deslocamento angular sempre que o eixo é movimentado depois do referencimaneto
int est_x = 0; //variável que indicará o estado do eixo x, pemitindo ou não que ele possa ser avançado depois de atingir o fim de curso
int est_y = 0; //variável que indicará o estado do eixo y, pemitindo ou não que ele possa ser avançado depois de atingir o fim de curso
int est_z = 0; //variável que indicará o estado do eixo z, pemitindo ou não que ele possa ser avançado depois de atingir o fim de curso
+float altura_solta = 50;
//pinos usados pelos botoes de fim de curso
DigitalIn fdc_x(PC_10);
@@ -29,7 +32,7 @@
InterruptIn interrupcao(PB_11);
//pinos do rele, saida digital para acionar o rele se necessario, dada uma entrada no pino da emergencia
-DigitalOut rele1(PC_5);
+DigitalOut pipeta(PC_5);
DigitalOut rele2(PB_9);
//botoes para movimentacao eixo a eixo
@@ -89,9 +92,42 @@
void mov_y_mais() {for(int i = 0; i < 4; i++){eixoy = hor[i];wait(t_motor);coord_y += (5.625*5/32)/360;est_y = 0;}}
void mov_z_mais() {for(int i = 0; i < 4; i++){eixoz = hor[i];wait(t_motor);coord_z += (5.625*5/32)/360;est_z = 0;}}
+void go_to_posicao(float posicao_x, float posicao_y){
+ while(coord_x < posicao_x){ //posicionar no local da pega salvo; - se for menor o valor da referencia, gira no sentido horário para aumentar
+ mov_x_mais();
+ }
+ while(coord_x > posicao_x){ //posicionar no local da pega salvo; - se for maior o valor da referencia, gira no sentido antihorário para diminuir
+ mov_x_menos();
+ }
+ while(coord_y < posicao_y){ //posicionar no local da pega salvo; - se for menor o valor da referencia, gira no sentido horário para aumentar
+ mov_y_mais();
+ }
+ while(coord_y > posicao_y){ //posicionar no local da pega salvo; - se for maior o valor da referencia, gira no sentido antihorário para diminuir
+ mov_y_menos();
+ };
+}
+
+float calcula_altura(int forma_do_pega, int dimensao, float v, int contador){
+ if(forma_do_pega==1){//quad
+ return (v-contador)/(dimensao*dimensao)*0.001-4;
+ }
+ else if(forma_do_pega==2){//red
+ return ((v-contador)/(PI*(dimensao/2)*(dimensao/2))*0.001-4);
+ };
+}
+
+void go_to_z(float posicao_z){
+ while(coord_z < posicao_z){ //posicionar no local da pega salvo; - se for menor o valor da referencia, gira no sentido horário para aumentar
+ mov_z_mais();
+ }
+ while(coord_z > posicao_z){ //posicionar no local da pega salvo; - se for maior o valor da referencia, gira no sentido antihorário para diminuir
+ mov_z_menos();
+ }
+}
+
//Funcao de interrupcao
void emergencia(){TextLCD lcd(D4, D5, D6, D7, D8, D9);
- rele1 = 0;
+ rele2 = 0;
eixox = 0b0000;
eixoy = 0b0000;
eixoz = 0b0000;
@@ -1243,6 +1279,198 @@
pos_y_solta[programa] = 10;
wait(3);
status_tela="solta";}
- if(pos_x_solta[programa]!=0 && pos_y_solta[programa]!=0 && volume_solta_9[programa]!=0 && dimensao_solta[programa]!=0 && status_tela[programa]=='solta'){funcao_acomp();status_tela="acompanhamento";}
+ //Ações para a tela acompanhamento (execução do programa)
+ //MAtriz 3x3 (6x6cm)
+ if(pos_x_solta[programa]!=0 && pos_y_solta[programa]!=0 && volume_solta_9[programa]!=0 && dimensao_solta[programa]!=0 && status_tela[programa]=='solta'){
+ funcao_acomp();
+ status_tela="acompanhamento";
+ int cont_pipeta = 0;
+ float altura;
+
+ float pos1[2] = {pos_x_solta[programa], pos_y_solta[programa]} ;
+ float pos2[2] = {pos_x_solta[programa]+2, pos_y_solta[programa]} ;
+ float pos3[2] = {pos_x_solta[programa]+4, pos_y_solta[programa]} ;
+ float pos4[2] = {pos_x_solta[programa], pos_y_solta[programa]+2} ;
+ float pos5[2] = {pos_x_solta[programa]+2, pos_y_solta[programa]+2} ;
+ float pos6[2] = {pos_x_solta[programa]+4, pos_y_solta[programa]+2} ;
+ float pos7[2] = {pos_x_solta[programa], pos_y_solta[programa]+4} ;
+ float pos8[2] = {pos_x_solta[programa]+2, pos_y_solta[programa]+4} ;
+ float pos9[2] = {pos_x_solta[programa]+4, pos_y_solta[programa]+4};
+
+ //Calcular a altura do pega com base no tipo do pega
+ for(int i; i<volume_solta_1[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ wait(1);
+ go_to_z(altura);
+ wait(1);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ wait(1);
+ go_to_z(normal_z);
+ wait(1);
+ go_to_posicao(pos1[0],pos1[1]);
+ wait(1);
+ go_to_z(altura_solta);
+ wait(1);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ wait(1);
+ go_to_z(normal_z);
+ wait(1);
+ cont_pipeta+=1;
+ }
+ for(int i; i<volume_solta_2[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ wait(1);
+ go_to_z(altura);
+ wait(1);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ wait(1);
+ go_to_z(normal_z);
+ wait(1);
+ go_to_posicao(pos2[0],pos2[1]);
+ wait(1);
+ go_to_z(altura_solta);
+ wait(1);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ wait(1);
+ go_to_z(normal_z);
+ wait(1);
+ cont_pipeta+=1;
+ }
+ for(int i; i<volume_solta_3[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ wait(1);
+ go_to_z(altura);
+ wait(1);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ wait(1);
+ go_to_z(normal_z);
+ wait(1);
+ go_to_posicao(pos3[0],pos3[1]);
+ wait(1);
+ go_to_z(altura_solta);
+ wait(1);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ wait(1);
+ go_to_z(normal_z);
+ wait(1);
+ cont_pipeta+=1;
+
+ }
+ for(int i; i<volume_solta_4[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ go_to_z(altura);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ go_to_posicao(pos4[0],pos4[1]);
+ go_to_z(altura_solta);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ cont_pipeta+=1;
+ }
+ for(int i; i<volume_solta_5[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ go_to_z(altura);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ go_to_posicao(pos5[0],pos5[1]);
+ go_to_z(altura_solta);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ cont_pipeta+=1;
+
+ }
+ for(int i; i<volume_solta_6[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ go_to_z(altura);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ go_to_posicao(pos6[0],pos6[1]);
+ go_to_z(altura_solta);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ cont_pipeta+=1;
+
+ }
+ for(int i; i<volume_solta_7[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ go_to_z(altura);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ go_to_posicao(pos7[0],pos7[1]);
+ go_to_z(altura_solta);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ cont_pipeta+=1;
+
+ }
+ for(int i; i<volume_solta_8[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ go_to_z(altura);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ go_to_posicao(pos8[0],pos8[1]);
+ go_to_z(altura_solta);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ cont_pipeta+=1;
+ }
+ for(int i; i<volume_solta_9[programa];i++){
+ altura = calcula_altura(tipo_pega[programa],medida_lado_pega[programa], volume_pega[programa], cont_pipeta);
+ go_to_posicao(pos_x_pega[programa], pos_y_pega[programa]);
+ go_to_z(altura);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ go_to_posicao(pos9[0],pos9[1]);
+ go_to_z(altura_solta);
+ pipeta = 0;
+ wait(1);
+ pipeta = 1;
+ go_to_z(normal_z);
+ cont_pipeta+=1;
+ }
+
+ }
}
}
\ No newline at end of file