Victor Mirkhan
Como o projeto está até agora....
Dependencies: Servo TCS3200 X_NUCLEO_IHM01A1 mbed TextLCD2
Fork of
QEI_HelloWorld
by 
Aaron Berk
Revision 4:5dd6e95b5706, committed 2017-06-05
- Comitter:
- Victor_Mirkhan
- Date:
- Mon Jun 05 21:52:27 2017 +0000
- Parent:
- 3:77d4b108568c
- Commit message:
- Atualiza??o;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 77d4b108568c -r 5dd6e95b5706 main.cpp
--- a/main.cpp Mon May 29 14:47:22 2017 +0000
+++ b/main.cpp Mon Jun 05 21:52:27 2017 +0000
@@ -23,7 +23,7 @@
InterruptIn confirma(PC_13); // Botão OK do menu (pino do botão do usuario)
InterruptIn back_btn(PB_7); // Botão para a função "voltar" do menu
-InterruptIn FDC(PC_11); // Fim de curso para o eixo Z
+InterruptIn FDC(PC_5); // Fim de curso para o eixo Z
InterruptIn FDC2(PC_8); // Fim de curso para o eixo X
InterruptIn FDC3(PC_6); // Fim de curso para o eixo Y
@@ -50,9 +50,6 @@
signed char ref_cursor = 0; // Numero de referência para a posição do cursor
unsigned char ref_menu = 0; // Numero para indicar o menu atual
-
-float conversao = 3/1600; // A variavel é definida para converter a posição de steps para mm (depende do micro step);
-
/*Criação de uma struct (basicamente, uma classe) para sets de velocidades e acelerações
diferentes. Cada objeto dessa struct possui os argumentos de velocidades max e min e
aceleração e desaceleração.
@@ -127,13 +124,6 @@
if (ref_cursor > 3) {
ref_cursor = 0;
}
-
- /* Opção para o menu com os dois tipos de ciclo. Apenas para impedir que
- o usuario coloque o cursor em um campo vazio. */
-
- if ((ref_cursor > 1) && (ref_menu == 2)) {
- ref_cursor = 0;
- }
}
void desce_cursor(void)
@@ -142,13 +132,6 @@
if (ref_cursor < 0) {
ref_cursor = 3;
}
-
- /* Opção para o menu com os dois tipos de ciclo. Apenas para impedir que
- o usuario coloque o cursor em um campo vazio. */
-
- if ((ref_cursor < 0) && (ref_menu == 2)) {
- ref_cursor = 1;
- }
}
/* FUNÇÃO "conclusao(char n)":
@@ -196,7 +179,7 @@
lcd.printf("Z: ");
}
-/*FUNÇÃO "menu_estatico(char n)":
+/*FUNÇÃO "menu_passivo(char n)":
-> A função apresenta um menu estático diferente de acordo com o valor n recebido.
Os menus aqui disponiveis são estáticos pois são temporários e não possibilitam
seleção de funções ou movimento do cursor.
@@ -204,7 +187,7 @@
função, da inicialização da maquina e seu estado de operação.
*/
-void menu_estatico(char n)
+void menu_passivo(char n)
{
switch(n) {
@@ -321,10 +304,10 @@
*/
void set_aceleracoesZ(int maxspeed,int minspeed,int ac,int dc)
{
- motorZ->set_max_speed(maxspeed);
- motorZ->set_min_speed(minspeed);
- motorZ->set_acceleration(ac);
- motorZ->set_deceleration(dc);
+ motorZ->set_max_speed(maxspeed)/2;
+ motorZ->set_min_speed(minspeed)/2;
+ motorZ->set_acceleration(ac)/2;
+ motorZ->set_deceleration(dc)/2;
}
void set_aceleracoesX(int maxspeed,int minspeed,int ac,int dc)
@@ -417,17 +400,14 @@
green = color.ReadGreen();
blue = color.ReadBlue();
clear = color.ReadClear();
-
- if((red<green)&&(red<blue)) {
-
+
+ if(green > 30) {
tagy = 2;
-
}
- if((green<red)&&(green<blue)&&(sensor)) {
-
+ if((green <= 30)&&(sensor)) {
tagy = 0;
}
- if((green<red)&&(green<blue)&&(!sensor)) {
+ if((green <= 30) && (!sensor)) {
tagy = 1;
}
@@ -456,34 +436,36 @@
return elemento;
}
-/* FUNÇÃO "seta_origem()":
+/* FUNÇÃO "sefta_origem()":
-> A função realiza o referenciamento completo para os 3 eixos, um de cada vez;
-> Por ser uma função mais longa, o processo é detalhado dentro da própria função;
*/
void seta_origem()
{
- menu_estatico(2);
+ menu_passivo(2);
//Seta uma alta velocidade para o referenciamento
- set_aceleracoesZ(3500,1000,100,100);
- set_aceleracoesX(3500,1000,100,100);
- set_aceleracoesY(3500,1000,100,100);
-
+ set_aceleracoesZ(3500/2,1200,100/2,100/2);
+ set_aceleracoesX(4000,3000,100,100);
+ set_aceleracoesY(4000,3000,100,100);
+
+ InterruptIn confirma(PC_13);
+ confirma.mode(PullUp);
+
while(1) {
-
- /*
- Motor continua andando em uma só direção enquanto variavel referZ estiver
- em FALSE
- */
+
+
+ InterruptIn FDC(PC_5);
+ FDC.fall(&motorZ_off);
+ FDC.mode(PullUp);
+
+ confirma.fall(&motorZ_off);
//Chamada do fim de curso para a função de interrupção
- FDC.fall(&motorZ_off);
- FDC2.fall(&motorX_off);
- FDC3.fall(&motorY_off);
-
+
if (referZ == false) {
motorZ->run(StepperMotor::BWD);
motorZ->wait_while_active();
@@ -503,16 +485,21 @@
}
}
-
+
while(1) {
-
+
// Motor continua andando em uma só direção enquanto variavel referX estiver
// em FALSE
-
+
+ InterruptIn FDC(PC_5);
+ FDC.fall(&motorX_off);
+ FDC.mode(PullUp);
+
if (referX == false) {
motorX->run(StepperMotor::BWD);
motorX->wait_while_active();
}
+
// Se a interrupção for chamada, a variavel referX se torna TRUE, acionando
// os comandos a seguir.
@@ -522,16 +509,20 @@
motorX->wait_while_active();
motorX->set_home(); // Seta posição de Home
int HomePosition = motorX->get_position();
- printf("Posicao Home = %d\r\n", HomePosition); //Verificar que HomePosition = 0
referX = false;
break; //Quebra do loop while, pois referenciamento do motor foi feito
}
}
-
- //referY = false; //Caso os botôes de fim de curso sejam apertados sem querer antes de seus zeramentos, tem certeza que vai entrar no loop, ja que são interrupçõs
+
+ referY = false; //Caso os botôes de fim de curso sejam apertados sem querer antes de seus zeramentos, tem certeza que vai entrar no loop, ja que são interrupçõs
+
while(1) {
-
+
+
+ InterruptIn FDC(PC_5);
+ FDC.fall(&motorY_off);
+ FDC.mode(PullUp);
//Motor continua andando em uma só direção enquanto variavel referX estiver
// em FALSE
@@ -573,6 +564,9 @@
if (change == 3) {
change = 0;
}
+ set_aceleracoesZ(set[change].maxspeed/2,set[change].minspeed/2,set[change].ac/2,set[change].dc/2);
+ set_aceleracoesX(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
+ set_aceleracoesY(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
switch(change) {
case 0: {
@@ -603,10 +597,6 @@
break;
}
}
-
- set_aceleracoesZ(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
- set_aceleracoesX(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
- set_aceleracoesY(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
}
@@ -620,7 +610,6 @@
dirx = true;
dirz = false;
diry = false;
- printf("EIXO X\r\n");
muda_velocidade();
}
@@ -629,7 +618,6 @@
dirx = false;
dirz = false;
diry = true;
- printf("EIXO Y\r\n");
muda_velocidade();
}
@@ -638,7 +626,6 @@
dirx = false;
dirz = true;
diry = false;
- printf("EIXO Z\r\n");
muda_velocidade();
}
@@ -779,20 +766,20 @@
back_btn.fall(&cycle_stop);
if (n == 0) {
- menu_estatico(3); // Ciclo infinito
+ menu_passivo(3); // Ciclo infinito
}
if (n == 1) {
- menu_estatico(4); // Ciclo único
+ menu_passivo(4); // Ciclo único
}
- unsigned char tag; // Tag que sairá como resultado da função de reconhecimento da peça
+ int tag; // Tag que sairá como resultado da função de reconhecimento da peça
printf("Comeco do ciclo!\r\n");
//Alta velocidade para retornar para a posição de home
- set_aceleracoesZ(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
+ set_aceleracoesZ(set[2].maxspeed/2,set[2].minspeed/2,set[2].ac/2,set[2].dc/2);
set_aceleracoesX(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
set_aceleracoesY(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
@@ -814,6 +801,8 @@
ref_cycle = true;
+ garra = 1;
+
while(1) {
// Vai para a posição de PICK
@@ -821,22 +810,22 @@
motorX->wait_while_active();
motorY->go_to(PickPos.posy);
motorY->wait_while_active();
- motorZ->go_to(PickPos.posz);
+ motorZ->go_to(PickPos.posz - 1000);
motorZ->wait_while_active();
tag = reconhecimento_peca(); // Reconhece a peça e qual posição de Drop irá
- garra = 1; // Abre a garra
- wait(1);
garra = 0.7; // Fecha a garra
wait(1);
// Vai para a posição de DROP
- motorZ->go_to(DropPos[tag].posx);
+ motorZ->move(StepperMotor::BWD,3000);
motorZ->wait_while_active();
- motorX->go_to(DropPos[tag].posy);
+ motorY->go_to(DropPos[tag].posy);
+ motorY->wait_while_active();
+ motorX->go_to(DropPos[tag].posx);
motorX->wait_while_active();
- motorY->go_to(DropPos[tag].posx);
- motorY->wait_while_active();
+ motorZ->go_to(DropPos[tag].posz);
+ motorZ->wait_while_active();
wait(1);
garra = 1; // Garra abre e deixa o objeto
@@ -862,6 +851,44 @@
também;
*/
+void paradaZ(void)
+{
+ motorZ->hard_stop();
+ wait(1);
+ int posicao = motorZ->get_position();
+ if (posicao < 0) {
+ motorZ->move(StepperMotor::FWD,2000);
+ }
+ else {
+ motorZ->move(StepperMotor::BWD,2000);
+ }
+}
+
+void paradaX(void)
+{
+ motorX->hard_stop();
+ wait(1);
+ int posicao = motorX->get_position();
+ if (posicao < 0) {
+ motorX->move(StepperMotor::FWD,3000);
+ }
+ else {
+ motorX->move(StepperMotor::BWD,3000);
+ }
+}
+void paradaY(void)
+{
+ motorY->hard_stop();
+ wait(1);
+ int posicao = motorY->get_position();
+ if (posicao < 0) {
+ motorY->move(StepperMotor::FWD,3000);
+ }
+ else {
+ motorY->move(StepperMotor::BWD,3000);
+ }
+}
+
void jog(char n) {
menu_jog();
lcd.locate(3,0);
@@ -913,29 +940,38 @@
btnX.fall(&ativa_eixoX);
btnY.fall(&ativa_eixoY);
btnZ.fall(&ativa_eixoZ);
-
+
while(save_pos == false) {
float posz = motorZ->get_position();
float posx = motorX->get_position();
float posy = motorY->get_position();
lcd.locate(3,1);
- lcd.printf("%.2f mm",posx*conversao);
+ lcd.printf("%.2f mm",posx*3/800);
lcd.locate(3,2);
- lcd.printf("%.2f mm",posy*conversao);
+ lcd.printf("%.2f mm",posy*3/800);
lcd.locate(3,3);
- lcd.printf("%.2f mm",posz*conversao);
+ lcd.printf("%.2f mm",posz*5/800);
if (jog_pos == true) {
if (dirx == true) {
+ InterruptIn FDC(PC_5);
+ FDC.mode(PullUp);
+ FDC.fall(¶daX);
motorX->run(StepperMotor::FWD);
motorX->wait_while_active();
}
if (diry == true) {
+ InterruptIn FDC(PC_5);
+ FDC.mode(PullUp);
+ FDC.fall(¶daY);
motorY->run(StepperMotor::FWD);
motorY->wait_while_active();
}
if (dirz == true) {
+ InterruptIn FDC(PC_5);
+ FDC.mode(PullUp);
+ FDC.fall(¶daZ);
motorZ->run(StepperMotor::FWD);
motorZ->wait_while_active();
}
@@ -943,14 +979,23 @@
if (jog_neg == true) {
if (dirx == true) {
+ InterruptIn FDC(PC_5);
+ FDC.mode(PullUp);
+ FDC.fall(¶daX);
motorX->run(StepperMotor::BWD);
motorX->wait_while_active();
}
if (diry == true) {
+ InterruptIn FDC(PC_5);
+ FDC.mode(PullUp);
+ FDC.fall(¶daY);
motorY->run(StepperMotor::BWD);
motorY->wait_while_active();
}
if (dirz == true) {
+ InterruptIn FDC(PC_5);
+ FDC.mode(PullUp);
+ FDC.fall(¶daZ);
motorZ->run(StepperMotor::BWD);
motorZ->wait_while_active();
}
@@ -970,10 +1015,9 @@
void back_op(void)
{
- if ((ref_menu == 1) || (ref_menu == 2)) {
- ref_cursor = 4; // O valor de ref_cursor em 4 chama um case que chama a primeira tela dinâmica;
- enable = true;
- }
+ ref_cursor = 4; // O valor de ref_cursor em 4 chama um case que chama a primeira tela dinâmica;
+ enable = true;
+ printf("BACK\r\n");
}
int main()
@@ -1029,7 +1073,7 @@
lcd.setBacklight(TextLCD::LightOn); // Liga o backlight do LCD
lcd.setAddress(0,0);
lcd.setCursor(TextLCD::CurOff_BlkOff); // Desliga o cursor para o menu estático
- menu_estatico(1);
+ menu_passivo(1);
lcd.setCursor(TextLCD::CurOn_BlkOn); // Liga o cursor novamente pro usuario poder mexe-lo
menu_dinamico(0);
@@ -1087,12 +1131,19 @@
btnY.mode(PullUp);
InterruptIn btnZ(PC_0); // Botão para selecionar o eixo Z
btnZ.mode(PullUp);
+
+ InterruptIn back_btn(PB_7);
+ back_btn.mode(PullUp);
+
confirma.fall(&selecao_funcao);
+
jog_button_pos.fall(&sobe_cursor);
jog_button_neg.fall(&desce_cursor);
+
btnX.fall(&ativa_eixoX);
btnY.fall(&ativa_eixoY);
btnZ.fall(&ativa_eixoZ);
+
do {
wait(0.1);
lcd.locate(0,ref_cursor); // Movimentação do cursor
@@ -1121,6 +1172,7 @@
}
case 2: {
// Operação de referenciamento
+ //int tag = reconhecimento_peca();
seta_origem();
flag = false;
break;
@@ -1136,7 +1188,9 @@
}
// Sub-menu para as posições a serem salvas
+
if(ref_menu == 1) {
+ back_btn.mode(PullUp);
back_btn.fall(&back_op);
if (enable == true) {
switch(ref_cursor) {
@@ -1144,24 +1198,36 @@
// Posição de Pick
jog(0);
flag = false;
+ InterruptIn back_btn(PB_7);
+ back_btn.mode(PullUp);
+ back_btn.fall(&back_op);
break;
}
case 1: {
// Posição de Drop 1
jog(1);
flag = false;
+ InterruptIn back_btn(PB_7);
+ back_btn.mode(PullUp);
+ back_btn.fall(&back_op);
break;
}
case 2: {
// Posição de Drop 2
jog(2);
flag = false;
+ InterruptIn back_btn(PB_7);
+ back_btn.mode(PullUp);
+ back_btn.fall(&back_op);
break;
}
case 3: {
// Posição de Drop 3
jog(3);
flag = false;
+ InterruptIn back_btn(PB_7);
+ back_btn.mode(PullUp);
+ back_btn.fall(&back_op);
break;
}
case 4: {
@@ -1179,6 +1245,7 @@
if (ref_menu == 2) {
back_btn.fall(&back_op);
+ back_btn.mode(PullUp);
if (enable == true) {
switch(ref_cursor) {
case 0: {