Projeto Mecatrônico / Mbed 2 deprecated CODIGO_FONTE

codigo fonte

Dependencies:   mbed TextLCD

main.cpp@1:7ed1c2479638, 2021-06-04 (annotated)

Committer:
doradalberto
Date:
Fri Jun 04 18:49:53 2021 +0000
Revision:
1:7ed1c2479638
Parent:
0:99c31d1bf4d3
Child:
2:706603d65abc
Erro na linha 556

Who changed what in which revision?

UserRevisionLine numberNew contents of line
diogonac 0:99c31d1bf4d3 1 #include "mbed.h"
diogonac 0:99c31d1bf4d3 2 #include "TextLCD.h"
diogonac 0:99c31d1bf4d3 3
diogonac 0:99c31d1bf4d3 4 TextLCD lcd(D8, D9, D4, D5, D6, D7); //Comunicação com LCD
diogonac 0:99c31d1bf4d3 5 BusOut motor_x(PC_8, PC_6, PC_5, PA_12); //Pinos para acionamento do motor do eixo X
diogonac 0:99c31d1bf4d3 6 BusOut motor_y(PA_11, PB_12, PB_11, PB_2); //Pinos para acionamento do motor do eixo Y
diogonac 0:99c31d1bf4d3 7 BusOut motor_z(PB_1, PB_15, PB_14, PB_13); //Pinos para acionamento do motor do eixo Z
diogonac 0:99c31d1bf4d3 8 DigitalOut sinal(PC_10); // Pino para o acionamento da pipeta
diogonac 0:99c31d1bf4d3 9 //DigitalIn botao_seleciona(PC_3); //botao de SELECT geral
diogonac 0:99c31d1bf4d3 10
diogonac 0:99c31d1bf4d3 11 Serial pc (D1, D0); //Comunicação com USB
diogonac 0:99c31d1bf4d3 12 AnalogIn joystick(PC_2); //Joystick potenciometro
diogonac 0:99c31d1bf4d3 13 //AnalogIn botao_SELECT(A0); //Botão SELECT da IHM
diogonac 0:99c31d1bf4d3 14 //Teste
diogonac 0:99c31d1bf4d3 15 InterruptIn botao_seleciona(PA_3); //Botão de salva posição Joystick
diogonac 0:99c31d1bf4d3 16
diogonac 0:99c31d1bf4d3 17 InterruptIn botao_emergencia_in(PC_4); //Botão de emergência para detectar acionamento
diogonac 0:99c31d1bf4d3 18 InterruptIn botao_fim_curso_rise(PC_12); //Chave de fim de curso
diogonac 0:99c31d1bf4d3 19
diogonac 0:99c31d1bf4d3 20
diogonac 0:99c31d1bf4d3 21 Timer debounce1; //Correção para botão de posição de salva
diogonac 0:99c31d1bf4d3 22 Timer debounce2; //Correção para botão de emergência
diogonac 0:99c31d1bf4d3 23 Timer debounce3; //Correção para chave de fim de curso
diogonac 0:99c31d1bf4d3 24
diogonac 0:99c31d1bf4d3 25 int pulsos = 0; //Contagem dos pulsos para o motor
diogonac 0:99c31d1bf4d3 26
diogonac 0:99c31d1bf4d3 27 int i; //Variável para contar o incremento no motor
diogonac 0:99c31d1bf4d3 28 int cont_fim_curso = 0; //Variável para contar o incremento do fim de curso
diogonac 0:99c31d1bf4d3 29 int J_XYZ; //Variável para ler o potenciometro do Joystick
diogonac 0:99c31d1bf4d3 30 int passo_fuso = 5; //Declara o passo do fuso
diogonac 0:99c31d1bf4d3 31 int pegaZ; //Variável que receberá o valor da posição de salva
diogonac 0:99c31d1bf4d3 32 int SELECT; //Variável para leitura do botão SELECT da IHM
diogonac 0:99c31d1bf4d3 33 int contador_SELECT = 0; //Define o contator do SELECT da IHM
diogonac 0:99c31d1bf4d3 34 int contador_emergencia = 0; //Define o contator do botão de emergência
diogonac 0:99c31d1bf4d3 35
diogonac 0:99c31d1bf4d3 36 int contador_rise = 0;
diogonac 0:99c31d1bf4d3 37 int seguranca_fdc = 0;
diogonac 0:99c31d1bf4d3 38
diogonac 0:99c31d1bf4d3 39 // ------------------Acionamento da Pipeta------------------
diogonac 0:99c31d1bf4d3 40 // Valores de Teste
diogonac 0:99c31d1bf4d3 41 int total_pega = 6; // quantidade de mL na posição de pega disponível
diogonac 0:99c31d1bf4d3 42 int quant_desejada = 3; // quantidade de mL na posição de solta 1
diogonac 0:99c31d1bf4d3 43 //int npos_solta = [1,2,3,4];
doradalberto 1:7ed1c2479638 44 int pos_solta_total = 4;
doradalberto 1:7ed1c2479638 45 float posZ_atual = 100;
doradalberto 1:7ed1c2479638 46 float posX_atual = 10;
doradalberto 1:7ed1c2479638 47 float posY_atual = 20;
diogonac 0:99c31d1bf4d3 48 float posX0_desejada = 0;
diogonac 0:99c31d1bf4d3 49 float posY0_desejada = 5;
diogonac 0:99c31d1bf4d3 50 float posZ0_desejada = 10;
diogonac 0:99c31d1bf4d3 51 float posX_desejada = 10;
diogonac 0:99c31d1bf4d3 52 float posY_desejada = 20;
diogonac 0:99c31d1bf4d3 53 float posZ_desejada = 90;
diogonac 0:99c31d1bf4d3 54
doradalberto 1:7ed1c2479638 55 float deslocamento_X = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 56 float deslocamento_Y = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 57 float deslocamento_Z = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 58
doradalberto 1:7ed1c2479638 59 float deslocamento_max_X = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 60 float deslocamento_max_Y = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 61 float deslocamento_max_Z = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 62
diogonac 0:99c31d1bf4d3 63 float velocidade = 0.003; //Armazena a velocidade dos motores
diogonac 0:99c31d1bf4d3 64
diogonac 0:99c31d1bf4d3 65 int atual_pega = total_pega; // "total_pega" é a quantidade inserida pelo usuário na posição de pega
diogonac 0:99c31d1bf4d3 66 //int quant_desejada; // "quant desejada" é a quantidade inserida pelo usuário na posição de solta
diogonac 0:99c31d1bf4d3 67 int quant_depositada = 0;
diogonac 0:99c31d1bf4d3 68 int n_pulsosX;
diogonac 0:99c31d1bf4d3 69 int n_pulsosY;
diogonac 0:99c31d1bf4d3 70 int n_pulsosZ;
diogonac 0:99c31d1bf4d3 71 int posZ_origem = 0;
diogonac 0:99c31d1bf4d3 72 int deslocamentoZ_partida = posZ_origem - deslocamento_Z; // deslocamento do eixo Z até a posição de partida (origem)
diogonac 0:99c31d1bf4d3 73 int deslocamento_X0 = posX0_desejada - posX_atual; // deslocamento do eixo X até a posição de pega desejada
diogonac 0:99c31d1bf4d3 74 int deslocamento_Y0 = posY0_desejada - posY_atual; // deslocamento do eixo Y até a posição de pega desejada
diogonac 0:99c31d1bf4d3 75 int deslocamento_Z0 = posZ0_desejada - posZ_atual; // deslocamento do eixo Z até a posição de pega desejada
diogonac 0:99c31d1bf4d3 76 int deslocamento_solta_X = posX_desejada - posX_atual; // deslocamento do eixo X até a posição de solta desejada
diogonac 0:99c31d1bf4d3 77 int deslocamento_solta_Y = posY_desejada - posY_atual; // deslocamento do eixo Y até a posição de solta desejada
diogonac 0:99c31d1bf4d3 78 int deslocamento_solta_Z = posZ_desejada - posZ_atual; // deslocamento do eixo Z até a posição de solta desejada
diogonac 0:99c31d1bf4d3 79
diogonac 0:99c31d1bf4d3 80 // ------------------Acionamento da Pipeta------------------
diogonac 0:99c31d1bf4d3 81
diogonac 0:99c31d1bf4d3 82 float tempo_horario, tempo_anti_horario; //Define os tempos de acionamento do motor
diogonac 0:99c31d1bf4d3 83 float passo_motor = 5.625/32; //Declara o passo angular do motor
diogonac 0:99c31d1bf4d3 84 float tempo = 1.5; // Define o tempo que a pipeta fica acionada
diogonac 0:99c31d1bf4d3 85
diogonac 0:99c31d1bf4d3 86 int pulsos_Z = 0; //Contagem dos pulsos para o motor_Z
diogonac 0:99c31d1bf4d3 87 int pulsos_X = 0; //Contagem dos pulsos para o motor_X
diogonac 0:99c31d1bf4d3 88 int pulsos_Y = 0; //Contagem dos pulsos para o motor_Y
diogonac 0:99c31d1bf4d3 89
diogonac 0:99c31d1bf4d3 90 int contador_botao_seleciona = 0; //Boleana para a lógica do botão SELECT do joystick
diogonac 0:99c31d1bf4d3 91
diogonac 0:99c31d1bf4d3 92 bool valor_SELECT = false; //Boleana para lógica do botão SELECT da IHM
diogonac 0:99c31d1bf4d3 93 bool estado_referenciamento = false; //Boleana para identificar a necessidade do referênciamento
diogonac 0:99c31d1bf4d3 94 bool estado_botao_emergencia_in = false; //Boleana para identificar a necessidade do referênciamentosatisfazer a lógica da rotina_emergencia_in
diogonac 0:99c31d1bf4d3 95 bool motor_referenciamento = false; //Boleana para permitir o movimento automático do motot no referenciamento
diogonac 0:99c31d1bf4d3 96
diogonac 0:99c31d1bf4d3 97 // Rotinas do programa
diogonac 0:99c31d1bf4d3 98 void rotina_posicao_salva (void);
diogonac 0:99c31d1bf4d3 99 void rotina_emergencia_in (void);
diogonac 0:99c31d1bf4d3 100 void rotina_velocidade_eixo_Z (void);
diogonac 0:99c31d1bf4d3 101 void rotina_deslocamento_eixo_Z (void);
diogonac 0:99c31d1bf4d3 102 void rotina_deslocamento_eixo_menos_Z (void);
diogonac 0:99c31d1bf4d3 103 void rotina_JOG (void);
diogonac 0:99c31d1bf4d3 104 void rotina_botoes_IHM (void);
diogonac 0:99c31d1bf4d3 105 void Rotina_Pega (void);
diogonac 0:99c31d1bf4d3 106 void Rotina_Solta (void);
diogonac 0:99c31d1bf4d3 107 void teste2dora (void);
diogonac 0:99c31d1bf4d3 108 void reposiciona_pipeta_origem (void);
diogonac 0:99c31d1bf4d3 109
diogonac 0:99c31d1bf4d3 110 void rotina_fdc_rise (void);
diogonac 0:99c31d1bf4d3 111 void referenciamento (void);
diogonac 0:99c31d1bf4d3 112
diogonac 0:99c31d1bf4d3 113 void rotina_estado_botao_seleciona (void);
diogonac 0:99c31d1bf4d3 114
diogonac 0:99c31d1bf4d3 115 int main()
diogonac 0:99c31d1bf4d3 116 {
diogonac 0:99c31d1bf4d3 117 pc.baud(115200); //Define a velocidade da porta USB
diogonac 0:99c31d1bf4d3 118 motor_x = 0x00; //Estabelece a condição inicial do motor do eixo X
diogonac 0:99c31d1bf4d3 119 motor_y = 0x00; //Estabelece a condição inicial do motor do eixo Y
diogonac 0:99c31d1bf4d3 120 motor_z = 0x00; //Estabelece a condição inicial do motor do eixo Z
diogonac 0:99c31d1bf4d3 121
diogonac 0:99c31d1bf4d3 122 //Declara os pinos de interrupção com suas funções
diogonac 0:99c31d1bf4d3 123 botao_seleciona.rise(&rotina_estado_botao_seleciona);
diogonac 0:99c31d1bf4d3 124
diogonac 0:99c31d1bf4d3 125 botao_emergencia_in.rise(&rotina_emergencia_in);
diogonac 0:99c31d1bf4d3 126 botao_fim_curso_rise.rise(&rotina_fdc_rise);
diogonac 0:99c31d1bf4d3 127
diogonac 0:99c31d1bf4d3 128
diogonac 0:99c31d1bf4d3 129 //Inicializa os timers
diogonac 0:99c31d1bf4d3 130 debounce1.start();
diogonac 0:99c31d1bf4d3 131 debounce2.start();
diogonac 0:99c31d1bf4d3 132 debounce3.start();
diogonac 0:99c31d1bf4d3 133
diogonac 0:99c31d1bf4d3 134 //Inicia o LCD
diogonac 0:99c31d1bf4d3 135 wait_ms(500);
diogonac 0:99c31d1bf4d3 136 lcd.locate(2,0);
diogonac 0:99c31d1bf4d3 137 lcd.printf("SELECT para");
diogonac 0:99c31d1bf4d3 138 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 139 lcd.printf("referenciamento");
diogonac 0:99c31d1bf4d3 140
diogonac 0:99c31d1bf4d3 141 while(1) {
diogonac 0:99c31d1bf4d3 142 //Leitura de variáveis
diogonac 0:99c31d1bf4d3 143 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 144 //SELECT = botao_SELECT.read_u16();
diogonac 0:99c31d1bf4d3 145 //pc.printf("estado_referenciamento=%4d, estado_botao_emergencia_in=%d valor_SELECT=%d\r\n", estado_referenciamento, estado_botao_emergencia_in, valor_SELECT);
diogonac 0:99c31d1bf4d3 146 //pc.printf("deslocamento_Z=%4f, pulsos=%d\r\n", deslocamento_Z, pulsos);
diogonac 0:99c31d1bf4d3 147 //pc.printf("J_XYZ= %d\r\n", J_XYZ);
diogonac 0:99c31d1bf4d3 148 if(estado_referenciamento == true & estado_botao_emergencia_in == false & contador_botao_seleciona == 2) {
diogonac 0:99c31d1bf4d3 149 //Condição para poder disponibilizar a função JOG
diogonac 0:99c31d1bf4d3 150 //Chama a rotina do JOG
diogonac 0:99c31d1bf4d3 151 rotina_JOG();
diogonac 0:99c31d1bf4d3 152 //LCD
diogonac 0:99c31d1bf4d3 153
diogonac 0:99c31d1bf4d3 154 lcd.locate(2,0);
diogonac 0:99c31d1bf4d3 155 lcd.printf("PosicaoZ:");
diogonac 0:99c31d1bf4d3 156 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 157 lcd.printf("%4fmm",deslocamento_Z);
diogonac 0:99c31d1bf4d3 158 //pc.printf("J_XYZ= %d\r\n", J_XYZ);
diogonac 0:99c31d1bf4d3 159 }
diogonac 0:99c31d1bf4d3 160
diogonac 0:99c31d1bf4d3 161 else {
diogonac 0:99c31d1bf4d3 162 //Chama a rotina dos botões da IHM
diogonac 0:99c31d1bf4d3 163 //rotina_botoes_IHM();
diogonac 0:99c31d1bf4d3 164
diogonac 0:99c31d1bf4d3 165 if(valor_SELECT == false) { //se estiver 0, mudar para 1 -- no PC da dora fica 0 pela LCD dela
diogonac 0:99c31d1bf4d3 166
diogonac 0:99c31d1bf4d3 167 lcd.cls();
diogonac 0:99c31d1bf4d3 168 lcd.locate(2,0);
diogonac 0:99c31d1bf4d3 169 lcd.printf("Iniciando o");
diogonac 0:99c31d1bf4d3 170 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 171 lcd.printf("referenciamento");
diogonac 0:99c31d1bf4d3 172
diogonac 0:99c31d1bf4d3 173 pc.printf("Referenciamento \r\n");
diogonac 0:99c31d1bf4d3 174
diogonac 0:99c31d1bf4d3 175 contador_SELECT = 1;
diogonac 0:99c31d1bf4d3 176 estado_referenciamento = false;
diogonac 0:99c31d1bf4d3 177 //Permite o movimento automático do motor até atingir o fim de curso
diogonac 0:99c31d1bf4d3 178 referenciamento();
diogonac 0:99c31d1bf4d3 179
diogonac 0:99c31d1bf4d3 180 pc.printf("Referenciamento feito\r\n");
diogonac 0:99c31d1bf4d3 181 lcd.cls();
diogonac 0:99c31d1bf4d3 182 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 183 lcd.printf("Pressione SELECT");
diogonac 0:99c31d1bf4d3 184 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 185 lcd.printf("para continuar");
diogonac 0:99c31d1bf4d3 186
diogonac 0:99c31d1bf4d3 187 }
diogonac 0:99c31d1bf4d3 188 }
diogonac 0:99c31d1bf4d3 189
diogonac 0:99c31d1bf4d3 190 //========================================================================================
diogonac 0:99c31d1bf4d3 191
diogonac 0:99c31d1bf4d3 192 //while (npos_solta < pos_solta_total) {
diogonac 0:99c31d1bf4d3 193 //checa se existem mais posições de solta
diogonac 0:99c31d1bf4d3 194
diogonac 0:99c31d1bf4d3 195 // if(contador_botao_seleciona == 2) {
diogonac 0:99c31d1bf4d3 196 // //pressiona para entrar nesse if
diogonac 0:99c31d1bf4d3 197 // // o usuario nao quer alterar nenhum parametro
diogonac 0:99c31d1bf4d3 198 // pc.printf("pressionado\r\n");
diogonac 0:99c31d1bf4d3 199 //
diogonac 0:99c31d1bf4d3 200 // quant_depositada = 0;
diogonac 0:99c31d1bf4d3 201 //
diogonac 0:99c31d1bf4d3 202 // while (quant_depositada < quant_desejada) {
diogonac 0:99c31d1bf4d3 203 // // "quant_desejada" definido pelo usuário na IHM
diogonac 0:99c31d1bf4d3 204 //
diogonac 0:99c31d1bf4d3 205 // if (atual_pega == 0) {
diogonac 0:99c31d1bf4d3 206 // lcd.cls();
diogonac 0:99c31d1bf4d3 207 // lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 208 // lcd.printf("Coleta vazia");
diogonac 0:99c31d1bf4d3 209 // lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 210 // lcd.printf("Pressione RST");
diogonac 0:99c31d1bf4d3 211 // wait(4);
diogonac 0:99c31d1bf4d3 212 // pc.printf("Coleta vazia \r\n");
diogonac 0:99c31d1bf4d3 213 // exit(0); // obriga o usuário a recomeçar
diogonac 0:99c31d1bf4d3 214 // }
diogonac 0:99c31d1bf4d3 215 // //rotina_referenciamento();
diogonac 0:99c31d1bf4d3 216 // //break;
diogonac 0:99c31d1bf4d3 217 // else {
diogonac 0:99c31d1bf4d3 218 // Rotina_Pega();
diogonac 0:99c31d1bf4d3 219 // Rotina_Solta();
diogonac 0:99c31d1bf4d3 220 // }
diogonac 0:99c31d1bf4d3 221 //
diogonac 0:99c31d1bf4d3 222 // atual_pega = atual_pega - 1;
diogonac 0:99c31d1bf4d3 223 // quant_depositada = quant_depositada + 1;
diogonac 0:99c31d1bf4d3 224 //
diogonac 0:99c31d1bf4d3 225 // pc.printf("atual_pega: %d \r\n",atual_pega);
diogonac 0:99c31d1bf4d3 226 // pc.printf("quant_depositada: %d \r\n",quant_depositada);
diogonac 0:99c31d1bf4d3 227 // }
diogonac 0:99c31d1bf4d3 228 // }
diogonac 0:99c31d1bf4d3 229 //}
diogonac 0:99c31d1bf4d3 230 // pc.printf("fora do if\r\n");
diogonac 0:99c31d1bf4d3 231 //rotina_referenciamento();
diogonac 0:99c31d1bf4d3 232 } //while(1)
diogonac 0:99c31d1bf4d3 233 } //int main()
diogonac 0:99c31d1bf4d3 234
diogonac 0:99c31d1bf4d3 235
diogonac 0:99c31d1bf4d3 236
diogonac 0:99c31d1bf4d3 237
diogonac 0:99c31d1bf4d3 238 void rotina_estado_botao_seleciona()
diogonac 0:99c31d1bf4d3 239 {
diogonac 0:99c31d1bf4d3 240 if(debounce1.read_ms() >= 10 & estado_referenciamento == true & estado_botao_emergencia_in == false) {
diogonac 0:99c31d1bf4d3 241 contador_botao_seleciona += 1;
diogonac 0:99c31d1bf4d3 242 pc.printf("contador_botao_seleciona= %d\r\n", contador_botao_seleciona);
diogonac 0:99c31d1bf4d3 243 lcd.cls();
diogonac 0:99c31d1bf4d3 244 }
diogonac 0:99c31d1bf4d3 245 debounce1.reset();
diogonac 0:99c31d1bf4d3 246 }
diogonac 0:99c31d1bf4d3 247
diogonac 0:99c31d1bf4d3 248 void rotina_emergencia_in()
diogonac 0:99c31d1bf4d3 249 {
diogonac 0:99c31d1bf4d3 250 if(debounce2.read_ms() >= 10) {
diogonac 0:99c31d1bf4d3 251
diogonac 0:99c31d1bf4d3 252 motor_x = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo X
diogonac 0:99c31d1bf4d3 253 motor_y = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Y
diogonac 0:99c31d1bf4d3 254 motor_z = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Z
diogonac 0:99c31d1bf4d3 255
diogonac 0:99c31d1bf4d3 256 estado_referenciamento = false; //Solicita um novo referenciamento
diogonac 0:99c31d1bf4d3 257 estado_botao_emergencia_in =! estado_botao_emergencia_in;
diogonac 0:99c31d1bf4d3 258 contador_emergencia += 1;
diogonac 0:99c31d1bf4d3 259
diogonac 0:99c31d1bf4d3 260 lcd.cls();
diogonac 0:99c31d1bf4d3 261 wait_ms(500);
diogonac 0:99c31d1bf4d3 262 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 263 lcd.printf("Emergencia ON");
diogonac 0:99c31d1bf4d3 264 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 265 lcd.printf("Pressione RST");
diogonac 0:99c31d1bf4d3 266
diogonac 0:99c31d1bf4d3 267 exit(0); //Encerra o programa e obriga o usuário a resetar o sistema
diogonac 0:99c31d1bf4d3 268
diogonac 0:99c31d1bf4d3 269 debounce2.reset();
diogonac 0:99c31d1bf4d3 270 }
diogonac 0:99c31d1bf4d3 271 }
diogonac 0:99c31d1bf4d3 272
diogonac 0:99c31d1bf4d3 273 void rotina_velocidade_eixo_Z()
diogonac 0:99c31d1bf4d3 274 {
diogonac 0:99c31d1bf4d3 275 //Converte a leitura do Joystick para tempo de acionamento do motor
diogonac 0:99c31d1bf4d3 276 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 277 tempo_horario = -1*J_XYZ*0.0000028 + 0.1875;
diogonac 0:99c31d1bf4d3 278 tempo_anti_horario = J_XYZ*0.000002785 + 0.0025;
diogonac 0:99c31d1bf4d3 279 }
diogonac 0:99c31d1bf4d3 280
diogonac 0:99c31d1bf4d3 281 void rotina_velocidade_referenciamento_eixo_Z()
diogonac 0:99c31d1bf4d3 282 {
diogonac 0:99c31d1bf4d3 283 //Converte a leitura do Joystick para tempo de acionamento do motor
diogonac 0:99c31d1bf4d3 284 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 285 tempo_horario = (-1*J_XYZ*0.0000028 + 0.1875)*2;
diogonac 0:99c31d1bf4d3 286 tempo_anti_horario = (J_XYZ*0.000002785 + 0.0025)*2;
diogonac 0:99c31d1bf4d3 287 }
diogonac 0:99c31d1bf4d3 288
diogonac 0:99c31d1bf4d3 289 void rotina_deslocamento_eixo()
diogonac 0:99c31d1bf4d3 290 {
diogonac 0:99c31d1bf4d3 291 //Calcula os deslocamentos no eixo Z
diogonac 0:99c31d1bf4d3 292
diogonac 0:99c31d1bf4d3 293 deslocamento_X = (passo_fuso*pulsos_X*passo_motor)/360;
diogonac 0:99c31d1bf4d3 294 deslocamento_Y = (passo_fuso*pulsos_Y*passo_motor)/360;
diogonac 0:99c31d1bf4d3 295 deslocamento_Z = (passo_fuso*pulsos_Z*passo_motor)/360;
diogonac 0:99c31d1bf4d3 296
diogonac 0:99c31d1bf4d3 297 // recebe_arduino.printf("X%3.2f\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 298 // recebe_arduino.printf("Y%3.2f\n", deslocamento_Y);
diogonac 0:99c31d1bf4d3 299 // recebe_arduino.printf("Z%3.2f\n", deslocamento_Z);
diogonac 0:99c31d1bf4d3 300
diogonac 0:99c31d1bf4d3 301 pc.printf("X%3.2f, Y%3.2f, Z%3.2f, J_XYZ=%d\n", deslocamento_X, deslocamento_Y, deslocamento_Z, J_XYZ);
diogonac 0:99c31d1bf4d3 302
diogonac 0:99c31d1bf4d3 303 }
diogonac 0:99c31d1bf4d3 304
diogonac 0:99c31d1bf4d3 305 //void rotina_botoes_IHM()
diogonac 0:99c31d1bf4d3 306 //{
diogonac 0:99c31d1bf4d3 307 // if(SELECT > 60000 & SELECT < 65000) valor_SELECT =! valor_SELECT;
diogonac 0:99c31d1bf4d3 308 // else valor_SELECT = 0;
diogonac 0:99c31d1bf4d3 309 //}
diogonac 0:99c31d1bf4d3 310
diogonac 0:99c31d1bf4d3 311 void referenciamento()
diogonac 0:99c31d1bf4d3 312 {
diogonac 0:99c31d1bf4d3 313
diogonac 0:99c31d1bf4d3 314 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_SELECT == 1) {//W1
diogonac 0:99c31d1bf4d3 315
diogonac 0:99c31d1bf4d3 316 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 317 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 318 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 319 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 320 wait(0.003);
diogonac 0:99c31d1bf4d3 321 }
diogonac 0:99c31d1bf4d3 322
diogonac 0:99c31d1bf4d3 323 if(contador_rise == 1) {//if1
diogonac 0:99c31d1bf4d3 324
diogonac 0:99c31d1bf4d3 325 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 1) {//W2
diogonac 0:99c31d1bf4d3 326
diogonac 0:99c31d1bf4d3 327 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 328 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 329 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 330 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 331 wait(0.003);
diogonac 0:99c31d1bf4d3 332
diogonac 0:99c31d1bf4d3 333 }
diogonac 0:99c31d1bf4d3 334
diogonac 0:99c31d1bf4d3 335 if(contador_rise == 2) {//if2
diogonac 0:99c31d1bf4d3 336
diogonac 0:99c31d1bf4d3 337 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 2) {//W3
diogonac 0:99c31d1bf4d3 338
diogonac 0:99c31d1bf4d3 339 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 340 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 341 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 342 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 343 wait(0.003);
diogonac 0:99c31d1bf4d3 344 }
diogonac 0:99c31d1bf4d3 345
diogonac 0:99c31d1bf4d3 346 if(contador_rise == 3) {//if3
diogonac 0:99c31d1bf4d3 347
diogonac 0:99c31d1bf4d3 348 while(seguranca_fdc != 8 & estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 3) {//W4
diogonac 0:99c31d1bf4d3 349 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 350 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 351 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 352 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 353 wait(0.003);
diogonac 0:99c31d1bf4d3 354
diogonac 0:99c31d1bf4d3 355 }
diogonac 0:99c31d1bf4d3 356 seguranca_fdc += 1;
diogonac 0:99c31d1bf4d3 357
diogonac 0:99c31d1bf4d3 358 }//Fecha W4
diogonac 0:99c31d1bf4d3 359
diogonac 0:99c31d1bf4d3 360 estado_referenciamento = true;
diogonac 0:99c31d1bf4d3 361 contador_SELECT = 0;
diogonac 0:99c31d1bf4d3 362 contador_rise = 0;
diogonac 0:99c31d1bf4d3 363 seguranca_fdc = 0;
diogonac 0:99c31d1bf4d3 364
diogonac 0:99c31d1bf4d3 365 valor_SELECT = true;
diogonac 0:99c31d1bf4d3 366 contador_botao_seleciona = 0;
diogonac 0:99c31d1bf4d3 367 deslocamento_X = 0; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 368 deslocamento_Y = 0; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 369 deslocamento_Z = 0; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 370 pulsos_X = 0;
diogonac 0:99c31d1bf4d3 371 pulsos_Y = 0;
diogonac 0:99c31d1bf4d3 372 pulsos_Z = 0;
diogonac 0:99c31d1bf4d3 373 deslocamento_max_X = 5; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 374 deslocamento_max_Y = 5; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 375 deslocamento_max_Z = 5; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 376 // velocidade = 0.1;
diogonac 0:99c31d1bf4d3 377
diogonac 0:99c31d1bf4d3 378 // recebe_arduino.printf("D\n"); //Done referenciamento
diogonac 0:99c31d1bf4d3 379
diogonac 0:99c31d1bf4d3 380 }//Fecha if3
diogonac 0:99c31d1bf4d3 381 }//Fecha W3
diogonac 0:99c31d1bf4d3 382
diogonac 0:99c31d1bf4d3 383 }//Fecha if2
diogonac 0:99c31d1bf4d3 384 }//Fecha W2
diogonac 0:99c31d1bf4d3 385 }//Fecha if1
diogonac 0:99c31d1bf4d3 386 }//Fecha W1
diogonac 0:99c31d1bf4d3 387 motor_x = 0x00;
diogonac 0:99c31d1bf4d3 388 motor_y = 0x00;
diogonac 0:99c31d1bf4d3 389 motor_z = 0x00;
diogonac 0:99c31d1bf4d3 390 }//Fecha funcao
diogonac 0:99c31d1bf4d3 391
diogonac 0:99c31d1bf4d3 392
diogonac 0:99c31d1bf4d3 393 void rotina_fdc_rise()
diogonac 0:99c31d1bf4d3 394 {
diogonac 0:99c31d1bf4d3 395 if(debounce3.read_ms() >= 50 & estado_botao_emergencia_in == false & estado_referenciamento == false & contador_SELECT == 1) {
diogonac 0:99c31d1bf4d3 396 contador_rise += 1;
diogonac 0:99c31d1bf4d3 397 pc.printf("contador_rise=%d\n", contador_rise);
diogonac 0:99c31d1bf4d3 398 debounce3.reset();
diogonac 0:99c31d1bf4d3 399 }
diogonac 0:99c31d1bf4d3 400 }
diogonac 0:99c31d1bf4d3 401
diogonac 0:99c31d1bf4d3 402
diogonac 0:99c31d1bf4d3 403 void rotina_JOG()
diogonac 0:99c31d1bf4d3 404 {
diogonac 0:99c31d1bf4d3 405 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 406
diogonac 0:99c31d1bf4d3 407 if(32400 <= J_XYZ & J_XYZ <= 34100) {
diogonac 0:99c31d1bf4d3 408 motor_x = 0x00;
diogonac 0:99c31d1bf4d3 409 motor_y = 0x00;
diogonac 0:99c31d1bf4d3 410 motor_z = 0x00;
diogonac 0:99c31d1bf4d3 411 }
diogonac 0:99c31d1bf4d3 412
diogonac 0:99c31d1bf4d3 413 else {
diogonac 0:99c31d1bf4d3 414 //============== Movimenta Eixo Y ======================================
diogonac 0:99c31d1bf4d3 415
diogonac 0:99c31d1bf4d3 416 if (J_XYZ > 34100 & deslocamento_Y > 0 & pulsos_Y > 0) {
diogonac 0:99c31d1bf4d3 417 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 418 // while(deslocamento_Y > 0) {
diogonac 0:99c31d1bf4d3 419 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 420 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 421 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 422 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 423 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 424 pulsos_Y-=1;
diogonac 0:99c31d1bf4d3 425 wait(velocidade);
diogonac 0:99c31d1bf4d3 426
diogonac 0:99c31d1bf4d3 427 }
diogonac 0:99c31d1bf4d3 428 // }
diogonac 0:99c31d1bf4d3 429 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 430 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 431 }
diogonac 0:99c31d1bf4d3 432
diogonac 0:99c31d1bf4d3 433 if(J_XYZ < 32400 & deslocamento_max_Y > deslocamento_Y & pulsos_Y < 2048) {
diogonac 0:99c31d1bf4d3 434 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 435 // while(deslocamento_max_Y > deslocamento_Y) {
diogonac 0:99c31d1bf4d3 436 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 437 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 438 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 439 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 440 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 441 pulsos_Y+=1;
diogonac 0:99c31d1bf4d3 442 wait(velocidade);
diogonac 0:99c31d1bf4d3 443
diogonac 0:99c31d1bf4d3 444 }
diogonac 0:99c31d1bf4d3 445 // }
diogonac 0:99c31d1bf4d3 446 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 447 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 448 }
diogonac 0:99c31d1bf4d3 449
diogonac 0:99c31d1bf4d3 450 //============== Movimenta Eixo X ======================================
diogonac 0:99c31d1bf4d3 451
diogonac 0:99c31d1bf4d3 452 if (J_XYZ > 34100 & deslocamento_X > 0 & pulsos_X > 0) {
diogonac 0:99c31d1bf4d3 453 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 454 // while(deslocamento_X > 0) {
diogonac 0:99c31d1bf4d3 455 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 456 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 457 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 458 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 459 pulsos_X-=1;
diogonac 0:99c31d1bf4d3 460 wait(velocidade);
diogonac 0:99c31d1bf4d3 461
diogonac 0:99c31d1bf4d3 462 }
diogonac 0:99c31d1bf4d3 463 // }
diogonac 0:99c31d1bf4d3 464 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 465 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 466 }
diogonac 0:99c31d1bf4d3 467
diogonac 0:99c31d1bf4d3 468 if(J_XYZ < 32400 & deslocamento_max_X > deslocamento_X & pulsos_X < 2048) {
diogonac 0:99c31d1bf4d3 469 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 470 // while(deslocamento_max_X > deslocamento_X) {
diogonac 0:99c31d1bf4d3 471 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 472 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 473 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 474 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 475 pulsos_X+=1;
diogonac 0:99c31d1bf4d3 476 wait(velocidade);
diogonac 0:99c31d1bf4d3 477
diogonac 0:99c31d1bf4d3 478 }
diogonac 0:99c31d1bf4d3 479 // }
diogonac 0:99c31d1bf4d3 480 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 481 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 482 }
diogonac 0:99c31d1bf4d3 483
diogonac 0:99c31d1bf4d3 484
diogonac 0:99c31d1bf4d3 485 //============== Movimenta Eixo Z ======================================
diogonac 0:99c31d1bf4d3 486
diogonac 0:99c31d1bf4d3 487 if (J_XYZ > 34100 & deslocamento_Z > 0 & pulsos_Z > 0) {
diogonac 0:99c31d1bf4d3 488 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 489 // while(deslocamento_Z > 0) {
diogonac 0:99c31d1bf4d3 490 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 491 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 492 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 493 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 494 pulsos_Z-=1;
diogonac 0:99c31d1bf4d3 495 wait(velocidade);
diogonac 0:99c31d1bf4d3 496
diogonac 0:99c31d1bf4d3 497 }
diogonac 0:99c31d1bf4d3 498 // }
diogonac 0:99c31d1bf4d3 499 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 500 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 501 }
diogonac 0:99c31d1bf4d3 502
diogonac 0:99c31d1bf4d3 503 if(J_XYZ < 32400 & deslocamento_max_Z > deslocamento_Z & pulsos_Z < 2048) {
diogonac 0:99c31d1bf4d3 504 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 505 // while(deslocamento_max_Z > deslocamento_Z) {
diogonac 0:99c31d1bf4d3 506 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 507 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 508 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 509 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 510 pulsos_Z+=1;
diogonac 0:99c31d1bf4d3 511 wait(velocidade);
diogonac 0:99c31d1bf4d3 512
diogonac 0:99c31d1bf4d3 513 }
diogonac 0:99c31d1bf4d3 514 // }
diogonac 0:99c31d1bf4d3 515 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 516 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 517 }
diogonac 0:99c31d1bf4d3 518
diogonac 0:99c31d1bf4d3 519
diogonac 0:99c31d1bf4d3 520 }
diogonac 0:99c31d1bf4d3 521 }
diogonac 0:99c31d1bf4d3 522
diogonac 0:99c31d1bf4d3 523
diogonac 0:99c31d1bf4d3 524 void Rotina_Pega()
diogonac 0:99c31d1bf4d3 525 {
diogonac 0:99c31d1bf4d3 526 // ------------ LEIA ----------
diogonac 0:99c31d1bf4d3 527 //Ele ta pulando o while do Z e X e indo direto pro Z -- ele não tá nem entrando nesse while
diogonac 0:99c31d1bf4d3 528 sinal = 0; // saída nível lógico "baixo"
diogonac 0:99c31d1bf4d3 529
doradalberto 1:7ed1c2479638 530 posZ_atual = deslocamento_Z; // posZ_atual = a posição de deslocamento neste momento
diogonac 0:99c31d1bf4d3 531 if(posZ_atual > posZ_origem) {
diogonac 0:99c31d1bf4d3 532 while (deslocamento_Z > posZ_origem) {
diogonac 0:99c31d1bf4d3 533 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 534 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 535 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 536 pulsos_Z -= 1;
diogonac 0:99c31d1bf4d3 537 wait(velocidade);
diogonac 0:99c31d1bf4d3 538 rotina_deslocamento_eixo();
doradalberto 1:7ed1c2479638 539 pc.printf("Pos atualZ AH: %f\r\n", deslocamento_Z);
diogonac 0:99c31d1bf4d3 540 }
diogonac 0:99c31d1bf4d3 541 }
diogonac 0:99c31d1bf4d3 542 }
diogonac 0:99c31d1bf4d3 543 else { //não faz nada
diogonac 0:99c31d1bf4d3 544
diogonac 0:99c31d1bf4d3 545 // while (deslocamento_Z < posZ0_desejada) {
diogonac 0:99c31d1bf4d3 546 // //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 547 // for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 548 // motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 549 // pulsos_Z -= 1
diogonac 0:99c31d1bf4d3 550 // wait(velocidade);
diogonac 0:99c31d1bf4d3 551 // rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 552 // pc.printf("Pos atualZ H: %d\r\n", deslocamento_Z);
doradalberto 1:7ed1c2479638 553 // }
diogonac 0:99c31d1bf4d3 554 }
diogonac 0:99c31d1bf4d3 555 }
doradalberto 1:7ed1c2479638 556 posX_atual = deslocamento_X;
diogonac 0:99c31d1bf4d3 557 if(posX_atual < posX0_desejada) {
diogonac 0:99c31d1bf4d3 558 while (deslocamento_X > posX0_desejada) {
diogonac 0:99c31d1bf4d3 559 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 560 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 561 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 562 pulsos_X += 1;
diogonac 0:99c31d1bf4d3 563 wait(velocidade);
diogonac 0:99c31d1bf4d3 564 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 565 pc.printf("Pos atualX AH: %d\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 566 }
diogonac 0:99c31d1bf4d3 567 }
diogonac 0:99c31d1bf4d3 568 } else {
diogonac 0:99c31d1bf4d3 569 while (deslocamento_X < posX0_desejada) {
diogonac 0:99c31d1bf4d3 570 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 571 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 572 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 573 pulsos_X -= 1;
diogonac 0:99c31d1bf4d3 574 wait(velocidade);
diogonac 0:99c31d1bf4d3 575 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 576 pc.printf("Pos atualX H: %d\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 577 }
diogonac 0:99c31d1bf4d3 578 }
diogonac 0:99c31d1bf4d3 579 }
doradalberto 1:7ed1c2479638 580 posY_atual = deslocamento_Y;
diogonac 0:99c31d1bf4d3 581 if(posY_atual < posY0_desejada) {
diogonac 0:99c31d1bf4d3 582 while (posY_atual > posY0_desejada) {
diogonac 0:99c31d1bf4d3 583 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 584 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 585 pulsos_Y += 1;
diogonac 0:99c31d1bf4d3 586 wait(velocidade);
diogonac 0:99c31d1bf4d3 587 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 588 }
diogonac 0:99c31d1bf4d3 589 }
diogonac 0:99c31d1bf4d3 590 }
diogonac 0:99c31d1bf4d3 591
diogonac 0:99c31d1bf4d3 592 else {
diogonac 0:99c31d1bf4d3 593 while (posY_atual < posY0_desejada) {
diogonac 0:99c31d1bf4d3 594 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 595 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 596 pulsos_Y -= 1;
diogonac 0:99c31d1bf4d3 597 wait(velocidade);
diogonac 0:99c31d1bf4d3 598 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 599 }
diogonac 0:99c31d1bf4d3 600 }
diogonac 0:99c31d1bf4d3 601 }
diogonac 0:99c31d1bf4d3 602
doradalberto 1:7ed1c2479638 603 posZ_atual = deslocamento_Z;
diogonac 0:99c31d1bf4d3 604 if(posZ_atual < posZ0_desejada) {
diogonac 0:99c31d1bf4d3 605 while (deslocamento_Z > posZ0_desejada) {
diogonac 0:99c31d1bf4d3 606 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 607 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 608 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 609 pulsos_Z += 1;
diogonac 0:99c31d1bf4d3 610 wait(velocidade);
diogonac 0:99c31d1bf4d3 611 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 612 pc.printf("Pos atualZ AH: %d\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 613 }
diogonac 0:99c31d1bf4d3 614 }
diogonac 0:99c31d1bf4d3 615 } else {
diogonac 0:99c31d1bf4d3 616 while (deslocamento_Z < posZ0_desejada) {
diogonac 0:99c31d1bf4d3 617 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 618 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 619 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 620 pulsos_Z -= 1;
diogonac 0:99c31d1bf4d3 621 wait(velocidade);
diogonac 0:99c31d1bf4d3 622 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 623 pc.printf("Pos atualX H: %d\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 624 }
diogonac 0:99c31d1bf4d3 625 }
diogonac 0:99c31d1bf4d3 626 }
diogonac 0:99c31d1bf4d3 627
diogonac 0:99c31d1bf4d3 628
diogonac 0:99c31d1bf4d3 629 sinal = 1; // saída nível lógico "alto"
diogonac 0:99c31d1bf4d3 630 lcd.cls();
diogonac 0:99c31d1bf4d3 631 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 632 lcd.printf("Coletando");
diogonac 0:99c31d1bf4d3 633 pc.printf("Coletando\r\n");
diogonac 0:99c31d1bf4d3 634 wait(tempo);
diogonac 0:99c31d1bf4d3 635
diogonac 0:99c31d1bf4d3 636 }
diogonac 0:99c31d1bf4d3 637
diogonac 0:99c31d1bf4d3 638 void Rotina_Solta()
diogonac 0:99c31d1bf4d3 639 {
diogonac 0:99c31d1bf4d3 640 sinal = 0; // saída nível lógico "baixo"
diogonac 0:99c31d1bf4d3 641
diogonac 0:99c31d1bf4d3 642 n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamentoZ_partida; // desloca a pipeta no eixo Z para sua posição de partida para evitar colisão
diogonac 0:99c31d1bf4d3 643
diogonac 0:99c31d1bf4d3 644 n_pulsosX = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_X; //movimentação do eixo X para posição de pega
diogonac 0:99c31d1bf4d3 645 n_pulsosY = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Y; //movimentação do eixo Y para posição de pega
diogonac 0:99c31d1bf4d3 646 n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Z; //movimentação do eixo Z para posição de pega
diogonac 0:99c31d1bf4d3 647
diogonac 0:99c31d1bf4d3 648 sinal = 1; // saída nível lógico "alto"
diogonac 0:99c31d1bf4d3 649 lcd.cls();
diogonac 0:99c31d1bf4d3 650 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 651 lcd.printf("Depositando");
diogonac 0:99c31d1bf4d3 652 pc.printf("Depositando\r\n");
diogonac 0:99c31d1bf4d3 653 wait(tempo);
diogonac 0:99c31d1bf4d3 654 }