Projeto Mecatrônico / Mbed 2 deprecated Referenciamento

Referenciamento do eixo Z

Dependencies:   mbed TextLCD

Referenciamneto.cpp@2:3eb5c816d496, 2021-05-21 (annotated)

Committer:
diogonac
Date:
Fri May 21 01:03:03 2021 +0000
Revision:
2:3eb5c816d496
Parent:
1:e8be78899409
Child:
3:8f569cc10b95
Atualizacao completa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
diogonac 0:7049aa99ce41 1 #include "mbed.h"
diogonac 0:7049aa99ce41 2 #include "TextLCD.h"
diogonac 0:7049aa99ce41 3
diogonac 1:e8be78899409 4 TextLCD lcd(D8, D9, D4, D5, D6, D7); //Comunicação com LCD
diogonac 1:e8be78899409 5 Serial pc (D1, D0); //Comunicação com USB
diogonac 1:e8be78899409 6 AnalogIn eixo_Z(PC_2); //Joystick eixo Z
diogonac 1:e8be78899409 7 AnalogIn botao_SELECT(A0); //Botão SELECT da IHM
diogonac 1:e8be78899409 8 InterruptIn posicao_salva(PA_15); //Botão de salva posição Joystick
diogonac 2:3eb5c816d496 9 InterruptIn botao_emergencia_in(PC_4); //Botão de emergência para detectar acionamento
diogonac 1:e8be78899409 10 InterruptIn botao_fim_curso(PC_12); //Chave de fim de curso
diogonac 1:e8be78899409 11 DigitalIn botao_indicador_fonte_externa(PC_10); //Indica se os componentes dependentes da fonte externa estão energizados
diogonac 0:7049aa99ce41 12
diogonac 1:e8be78899409 13 BusOut motor_z(PB_1, PB_15, PB_14, PB_13); //Pinos para acionamento do motor
diogonac 1:e8be78899409 14 Timer debounce1;
diogonac 1:e8be78899409 15 Timer debounce2;
diogonac 1:e8be78899409 16 Timer debounce3;
diogonac 1:e8be78899409 17 Timer debounce4;
diogonac 1:e8be78899409 18
diogonac 0:7049aa99ce41 19 float tempo_horario, tempo_anti_horario; //Segundos
diogonac 0:7049aa99ce41 20 int pulsos_horario, pulsos_anti_horario;
diogonac 0:7049aa99ce41 21 int i;
diogonac 0:7049aa99ce41 22 int J_Z;
diogonac 2:3eb5c816d496 23 float deslocamento_horario_Z = 0;
diogonac 2:3eb5c816d496 24 float deslocamento_anti_horario_Z = 0;
diogonac 2:3eb5c816d496 25 float deslocamento_Z = 0;
diogonac 2:3eb5c816d496 26 float deslocamento_max_Z = 0;
diogonac 2:3eb5c816d496 27
diogonac 2:3eb5c816d496 28 float passo_motor = 22.5;//5.625/32;
diogonac 0:7049aa99ce41 29 int passo_fuso = 5;
diogonac 0:7049aa99ce41 30 int pegaZ;
diogonac 1:e8be78899409 31 int posicao_salva_estado;
diogonac 0:7049aa99ce41 32 int fonte_externa;
diogonac 1:e8be78899409 33 int SELECT;
diogonac 1:e8be78899409 34 int contador_SELECT = 0;
diogonac 2:3eb5c816d496 35 int contador_emergencia = 0;
diogonac 1:e8be78899409 36 bool valor_SELECT = false;
diogonac 1:e8be78899409 37 bool estado_referenciamento = false;
diogonac 2:3eb5c816d496 38 bool estado_botao_emergencia_in = false;
diogonac 2:3eb5c816d496 39 bool motor_referenciamento = false;
diogonac 2:3eb5c816d496 40
diogonac 2:3eb5c816d496 41
diogonac 0:7049aa99ce41 42 void rotina_posicao_salva(void);
diogonac 2:3eb5c816d496 43 void rotina_emergencia_in(void);
diogonac 1:e8be78899409 44 void rotina_velocidade_eixo_Z(void);
diogonac 1:e8be78899409 45 void rotina_deslocamento_eixo_Z (void);
diogonac 1:e8be78899409 46 void rotina_JOG_Z (void);
diogonac 1:e8be78899409 47 void rotina_botoes_IHM (void);
diogonac 1:e8be78899409 48 void rotina_referenciamento (void);
diogonac 1:e8be78899409 49 void rotina_aciona_motor (void);
diogonac 0:7049aa99ce41 50
diogonac 0:7049aa99ce41 51
diogonac 0:7049aa99ce41 52 int main()
diogonac 0:7049aa99ce41 53 {
diogonac 0:7049aa99ce41 54
diogonac 0:7049aa99ce41 55 pc.baud(115200);
diogonac 0:7049aa99ce41 56 motor_z = 0x00;
diogonac 0:7049aa99ce41 57 posicao_salva.rise(&rotina_posicao_salva);
diogonac 2:3eb5c816d496 58 botao_emergencia_in.rise(&rotina_emergencia_in);
diogonac 1:e8be78899409 59 botao_fim_curso.rise(&rotina_referenciamento);
diogonac 1:e8be78899409 60 wait_ms(500);
diogonac 1:e8be78899409 61 lcd.locate(2,0);
diogonac 1:e8be78899409 62 lcd.printf("SELECT para");
diogonac 1:e8be78899409 63 lcd.locate(0,1);
diogonac 1:e8be78899409 64 lcd.printf("referenciamento");
diogonac 0:7049aa99ce41 65
diogonac 0:7049aa99ce41 66 while(1) {
diogonac 0:7049aa99ce41 67
diogonac 0:7049aa99ce41 68 J_Z = eixo_Z.read_u16();
diogonac 0:7049aa99ce41 69 fonte_externa = botao_indicador_fonte_externa.read();
diogonac 1:e8be78899409 70 SELECT = botao_SELECT.read_u16();
diogonac 0:7049aa99ce41 71
diogonac 2:3eb5c816d496 72 // pc.printf("PosicaoZ=%4f,Posicao_Salva=%d\r\n", deslocamento_Z, pegaZ);
diogonac 2:3eb5c816d496 73 // pc.printf("Z=%4d, PegaZ=%4d, Posicao_salva_estado=%d, Botao_emergencia_estado=%d, Tempo1=%4f, Tempo2=%4f, Pulsos=%d, Deslocamento_Z=%f, Velocidade_Z=%f\r\n", J_Z, pegaZ, posicao_salva_estado, botao_emergencia_estado, tempo_horario, tempo_anti_horario, pulsos_anti_horario, deslocamento_Z, velocidade_Z);
diogonac 2:3eb5c816d496 74 pc.printf("estado_referenciamento=%d, estado_botao_emergencia_in=%d, contador_emergencia=%d\r\n", estado_referenciamento, estado_botao_emergencia_in, contador_emergencia);
diogonac 2:3eb5c816d496 75 if(fonte_externa == 0 & estado_referenciamento == true & estado_botao_emergencia_in == true) {
diogonac 2:3eb5c816d496 76
diogonac 1:e8be78899409 77 lcd.locate(2,0);
diogonac 1:e8be78899409 78 lcd.printf("PosicaoZ:");
diogonac 0:7049aa99ce41 79 lcd.locate(0,1);
diogonac 2:3eb5c816d496 80 lcd.printf("%4fmm",deslocamento_Z);
diogonac 1:e8be78899409 81 // pc.printf("Fonte=%d\r\n", fonte_externa);
diogonac 1:e8be78899409 82 rotina_JOG_Z();
diogonac 0:7049aa99ce41 83
diogonac 1:e8be78899409 84 } else {
diogonac 1:e8be78899409 85 rotina_botoes_IHM();
diogonac 1:e8be78899409 86 if(valor_SELECT == 1) {
diogonac 1:e8be78899409 87 contador_SELECT = 1;
diogonac 1:e8be78899409 88 lcd.cls();
diogonac 1:e8be78899409 89 lcd.locate(2,0);
diogonac 1:e8be78899409 90 lcd.printf("Iniciando");
diogonac 1:e8be78899409 91 lcd.locate(0,1);
diogonac 1:e8be78899409 92 lcd.printf("referenciamento");
diogonac 2:3eb5c816d496 93 while(motor_referenciamento == false){
diogonac 2:3eb5c816d496 94 for(i = 0; i < 4; i++) {
diogonac 2:3eb5c816d496 95 motor_z = 1 << i;
diogonac 2:3eb5c816d496 96 //rotina_velocidade_eixo_Z();
diogonac 2:3eb5c816d496 97 //rotina_deslocamento_eixo_Z();
diogonac 2:3eb5c816d496 98 wait(0.003);
diogonac 2:3eb5c816d496 99 //pulsos_horario+=1;
diogonac 2:3eb5c816d496 100 }
diogonac 2:3eb5c816d496 101
diogonac 2:3eb5c816d496 102 }
diogonac 1:e8be78899409 103 pc.printf("estado_referenciamento=%d\r\n", estado_referenciamento);
diogonac 1:e8be78899409 104 }
diogonac 0:7049aa99ce41 105 }
diogonac 0:7049aa99ce41 106 }
diogonac 0:7049aa99ce41 107 }
diogonac 0:7049aa99ce41 108
diogonac 0:7049aa99ce41 109
diogonac 0:7049aa99ce41 110 void rotina_posicao_salva()
diogonac 0:7049aa99ce41 111 {
diogonac 1:e8be78899409 112 debounce1.start();
diogonac 2:3eb5c816d496 113 if(debounce1.read_ms() >= 10) pegaZ = deslocamento_Z;
diogonac 0:7049aa99ce41 114 else pegaZ = 0;
diogonac 1:e8be78899409 115 debounce1.reset();
diogonac 0:7049aa99ce41 116 }
diogonac 0:7049aa99ce41 117
diogonac 2:3eb5c816d496 118 void rotina_emergencia_in()
diogonac 0:7049aa99ce41 119 {
diogonac 1:e8be78899409 120 debounce2.start();
diogonac 2:3eb5c816d496 121 if(debounce2.read_ms() >= 120) {
diogonac 2:3eb5c816d496 122
diogonac 1:e8be78899409 123 motor_z = 0x00;
diogonac 1:e8be78899409 124 estado_referenciamento = false;
diogonac 2:3eb5c816d496 125 estado_botao_emergencia_in =! estado_botao_emergencia_in;
diogonac 2:3eb5c816d496 126 contador_emergencia+=1;
diogonac 1:e8be78899409 127
diogonac 1:e8be78899409 128 }
diogonac 1:e8be78899409 129 debounce2.reset();
diogonac 2:3eb5c816d496 130 if(contador_emergencia > 1) {
diogonac 2:3eb5c816d496 131 lcd.cls();
diogonac 2:3eb5c816d496 132 wait_ms(500);
diogonac 2:3eb5c816d496 133 lcd.locate(0,0);
diogonac 2:3eb5c816d496 134 lcd.printf("Emergencia ON");
diogonac 2:3eb5c816d496 135 lcd.locate(0,1);
diogonac 2:3eb5c816d496 136 lcd.printf("Pressione RST");
diogonac 2:3eb5c816d496 137 exit(0);
diogonac 2:3eb5c816d496 138 }
diogonac 0:7049aa99ce41 139 }
diogonac 0:7049aa99ce41 140
diogonac 2:3eb5c816d496 141
diogonac 2:3eb5c816d496 142
diogonac 1:e8be78899409 143 void rotina_velocidade_eixo_Z()
diogonac 0:7049aa99ce41 144 {
diogonac 2:3eb5c816d496 145 tempo_horario = -1*J_Z*0.0000028 + 0.1875;
diogonac 2:3eb5c816d496 146 tempo_anti_horario = J_Z*0.000002785 + 0.0025;
diogonac 0:7049aa99ce41 147 }
diogonac 0:7049aa99ce41 148
diogonac 1:e8be78899409 149 void rotina_deslocamento_eixo_Z()
diogonac 0:7049aa99ce41 150 {
diogonac 0:7049aa99ce41 151 deslocamento_horario_Z = (passo_fuso*pulsos_horario*passo_motor)/360;
diogonac 0:7049aa99ce41 152 deslocamento_anti_horario_Z = (passo_fuso*pulsos_anti_horario*passo_motor)/360;
diogonac 2:3eb5c816d496 153 deslocamento_Z = deslocamento_anti_horario_Z - deslocamento_horario_Z;
diogonac 0:7049aa99ce41 154 }
diogonac 0:7049aa99ce41 155
diogonac 1:e8be78899409 156 void rotina_botoes_IHM()
diogonac 1:e8be78899409 157 {
diogonac 1:e8be78899409 158 if(SELECT > 60000 & SELECT < 65000) valor_SELECT =! valor_SELECT;
diogonac 1:e8be78899409 159 else valor_SELECT = 0;
diogonac 1:e8be78899409 160 }
diogonac 1:e8be78899409 161
diogonac 1:e8be78899409 162 void rotina_referenciamento()
diogonac 1:e8be78899409 163 {
diogonac 1:e8be78899409 164
diogonac 1:e8be78899409 165 debounce3.start();
diogonac 2:3eb5c816d496 166 if(debounce3.read_ms() >= 10 & contador_SELECT == 1 & estado_botao_emergencia_in == true) {
diogonac 1:e8be78899409 167 estado_referenciamento =! estado_referenciamento;
diogonac 2:3eb5c816d496 168 motor_referenciamento =! motor_referenciamento;
diogonac 2:3eb5c816d496 169 deslocamento_max_Z = 100;
diogonac 1:e8be78899409 170 lcd.cls();
diogonac 1:e8be78899409 171 wait_ms(50);
diogonac 1:e8be78899409 172 lcd.locate(2,0);
diogonac 1:e8be78899409 173 lcd.printf("Detectado");
diogonac 1:e8be78899409 174 wait_ms(1500);
diogonac 1:e8be78899409 175 lcd.cls();
diogonac 1:e8be78899409 176 contador_SELECT = 0;
diogonac 1:e8be78899409 177 }
diogonac 1:e8be78899409 178 debounce3.reset();
diogonac 1:e8be78899409 179
diogonac 1:e8be78899409 180
diogonac 1:e8be78899409 181 }
diogonac 1:e8be78899409 182
diogonac 1:e8be78899409 183
diogonac 1:e8be78899409 184 void rotina_aciona_motor_horario()
diogonac 1:e8be78899409 185 {
diogonac 1:e8be78899409 186
diogonac 1:e8be78899409 187
diogonac 1:e8be78899409 188
diogonac 1:e8be78899409 189
diogonac 1:e8be78899409 190 }
diogonac 1:e8be78899409 191
diogonac 1:e8be78899409 192
diogonac 1:e8be78899409 193 void rotina_JOG_Z()
diogonac 0:7049aa99ce41 194 {
diogonac 0:7049aa99ce41 195
diogonac 0:7049aa99ce41 196 if(31000 <= J_Z & J_Z <= 35000) {
diogonac 0:7049aa99ce41 197
diogonac 0:7049aa99ce41 198 motor_z = 0x00;
diogonac 0:7049aa99ce41 199
diogonac 0:7049aa99ce41 200 } else {
diogonac 2:3eb5c816d496 201 if (J_Z > 31000 & deslocamento_Z > 0) {
diogonac 0:7049aa99ce41 202 for(i = 0; i < 4; i++) {
diogonac 0:7049aa99ce41 203 motor_z = 1 << i;
diogonac 1:e8be78899409 204 rotina_velocidade_eixo_Z();
diogonac 1:e8be78899409 205 rotina_deslocamento_eixo_Z();
diogonac 0:7049aa99ce41 206 wait(tempo_horario);
diogonac 0:7049aa99ce41 207 pulsos_horario+=1;
diogonac 0:7049aa99ce41 208
diogonac 0:7049aa99ce41 209 }
diogonac 0:7049aa99ce41 210 }
diogonac 2:3eb5c816d496 211 else motor_z = 0x00;
diogonac 0:7049aa99ce41 212
diogonac 2:3eb5c816d496 213 if(J_Z < 35000 & deslocamento_Z < deslocamento_max_Z) {
diogonac 0:7049aa99ce41 214 for(i = 3; i > -1; i--) {
diogonac 0:7049aa99ce41 215 motor_z = 1 << i;
diogonac 1:e8be78899409 216 rotina_velocidade_eixo_Z();
diogonac 1:e8be78899409 217 rotina_deslocamento_eixo_Z();
diogonac 0:7049aa99ce41 218 wait(tempo_anti_horario);
diogonac 0:7049aa99ce41 219 pulsos_anti_horario+=1;
diogonac 0:7049aa99ce41 220
diogonac 0:7049aa99ce41 221 }
diogonac 0:7049aa99ce41 222 }
diogonac 2:3eb5c816d496 223 else motor_z = 0x00;
diogonac 0:7049aa99ce41 224 }
diogonac 0:7049aa99ce41 225
diogonac 0:7049aa99ce41 226
diogonac 0:7049aa99ce41 227 }