Stephanie Liu / Mbed 2 deprecated TESTE_1

grupo 3 - pipfox projeto mecatronico 2021

Dependencies:   mbed TouchScreen_kbv_mbed MCUFRIEND_kbv_R299 GLUE_STUFF_kbv ADA_GFX_kbv_R1107

funcao.cpp@5:fa6ff327713c, 2021-06-01 (annotated)

Committer:
stephanie_liu
Date:
Tue Jun 01 14:33:34 2021 +0000
Revision:
5:fa6ff327713c
Grupo 3 - PipFox - Codigo desenvolvimento

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stephanie_liu 5:fa6ff327713c 1 // ------ Funcoes --------
stephanie_liu 5:fa6ff327713c 2 // -------------- Importando o arquivo de setup --------------
stephanie_liu 5:fa6ff327713c 3 #include "setup.h"
stephanie_liu 5:fa6ff327713c 4
stephanie_liu 5:fa6ff327713c 5 // --------------------- acionamento motor ---------------------------
stephanie_liu 5:fa6ff327713c 6 void aciona_motor(int tempo, bool sentido, BusOut &motor) {
stephanie_liu 5:fa6ff327713c 7 if (sentido) {
stephanie_liu 5:fa6ff327713c 8 for (int i = 0; i < 4; i++) {
stephanie_liu 5:fa6ff327713c 9 motor = 1 << i; // verificar se o estado do sistema eh 0 ou 1
stephanie_liu 5:fa6ff327713c 10 wait_ms(tempo);
stephanie_liu 5:fa6ff327713c 11 }
stephanie_liu 5:fa6ff327713c 12 } else {
stephanie_liu 5:fa6ff327713c 13 for (int i = 3; i > -1; i--) {
stephanie_liu 5:fa6ff327713c 14 motor = 1 << i;
stephanie_liu 5:fa6ff327713c 15 wait_ms(tempo);
stephanie_liu 5:fa6ff327713c 16 }
stephanie_liu 5:fa6ff327713c 17 }
stephanie_liu 5:fa6ff327713c 18 }
stephanie_liu 5:fa6ff327713c 19
stephanie_liu 5:fa6ff327713c 20 void desliga_motor(BusOut &motor) { motor = 0, 0, 0, 0; }
stephanie_liu 5:fa6ff327713c 21
stephanie_liu 5:fa6ff327713c 22 void Controlador::variavel_default() {
stephanie_liu 5:fa6ff327713c 23 for (int i = 0; i < 3; i++) {
stephanie_liu 5:fa6ff327713c 24 ref_feito[i] = false;
stephanie_liu 5:fa6ff327713c 25 coleta[i] = 0;
stephanie_liu 5:fa6ff327713c 26 atual[i] = 0;
stephanie_liu 5:fa6ff327713c 27 distancia_coleta_atual[i] = 0;
stephanie_liu 5:fa6ff327713c 28 distancia_solta_coleta[i] = 0;
stephanie_liu 5:fa6ff327713c 29 step[i] = 0;
stephanie_liu 5:fa6ff327713c 30 step_rev[i] = 512;
stephanie_liu 5:fa6ff327713c 31 }
stephanie_liu 5:fa6ff327713c 32 passo[0] = 3;
stephanie_liu 5:fa6ff327713c 33 passo[1] = 3;
stephanie_liu 5:fa6ff327713c 34 passo[2] = 10;
stephanie_liu 5:fa6ff327713c 35 enable = true; // 0 -> emergencia; -> 1 funcionamento normal; 2-> fim do processo
stephanie_liu 5:fa6ff327713c 36 emergencia = false;
stephanie_liu 5:fa6ff327713c 37 soltas = 0;
stephanie_liu 5:fa6ff327713c 38 tempo = 3;
stephanie_liu 5:fa6ff327713c 39 }
stephanie_liu 5:fa6ff327713c 40
stephanie_liu 5:fa6ff327713c 41 void Controlador::emerg() {
stephanie_liu 5:fa6ff327713c 42 for (int i = 0; i < 3; i++) {
stephanie_liu 5:fa6ff327713c 43 step[i] = 0;
stephanie_liu 5:fa6ff327713c 44 ref_feito[i] = false;
stephanie_liu 5:fa6ff327713c 45 }
stephanie_liu 5:fa6ff327713c 46 enable = false;
stephanie_liu 5:fa6ff327713c 47 emergencia = true;
stephanie_liu 5:fa6ff327713c 48 }
stephanie_liu 5:fa6ff327713c 49
stephanie_liu 5:fa6ff327713c 50 //void Controlador::display() {
stephanie_liu 5:fa6ff327713c 51 // tft.setTextColor(GREEN);
stephanie_liu 5:fa6ff327713c 52 // tft.setTextSize(3);
stephanie_liu 5:fa6ff327713c 53 // tft.setCursor(0, 10); // Orientação do texto X,Y
stephanie_liu 5:fa6ff327713c 54 // int dist_x = step[0] * passo[0] / step_rev[0];
stephanie_liu 5:fa6ff327713c 55 // int dist_y = step[1] * passo[1] / step_rev[1];
stephanie_liu 5:fa6ff327713c 56 // int dist_z = step[2] * passo[2] / step_rev[2];
stephanie_liu 5:fa6ff327713c 57 // tft.printf("distancia_x=%.2f\ndistancia_y=%.2f\ndistancia_z=%.f\npassos_x=%."
stephanie_liu 5:fa6ff327713c 58 // "0f\npassos_y=%.0f",
stephanie_liu 5:fa6ff327713c 59 // dist_x, dist_y, dist_z, step[0], step[1]);
stephanie_liu 5:fa6ff327713c 60 // tft.printf("\npassos_z=%.0f", step[2]);
stephanie_liu 5:fa6ff327713c 61 //}
stephanie_liu 5:fa6ff327713c 62
stephanie_liu 5:fa6ff327713c 63 void Controlador::eixo_refere() {
stephanie_liu 5:fa6ff327713c 64 //BusOut motores[3] = {motor_x, motor_y, motor_z};
stephanie_liu 5:fa6ff327713c 65 for (int i = 0; i < 3; i++) {
stephanie_liu 5:fa6ff327713c 66 //pc.printf("referenciando eixo %d\r\n", i);
stephanie_liu 5:fa6ff327713c 67 bool finding_max = true;
stephanie_liu 5:fa6ff327713c 68 while (!ref_feito[i]) {
stephanie_liu 5:fa6ff327713c 69 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 70 bool first_read = endstops.read();
stephanie_liu 5:fa6ff327713c 71 if (finding_max) {
stephanie_liu 5:fa6ff327713c 72 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 73 aciona_motor(tempo, true, motores[i]);
stephanie_liu 5:fa6ff327713c 74 wait_ms(3);
stephanie_liu 5:fa6ff327713c 75 bool bateu = first_read && endstops.read();
stephanie_liu 5:fa6ff327713c 76 if (bateu) {
stephanie_liu 5:fa6ff327713c 77 finding_max = false;
stephanie_liu 5:fa6ff327713c 78 max_coord[i] = step[i];
stephanie_liu 5:fa6ff327713c 79 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 80 aciona_motor(10, true, motores[i]);
stephanie_liu 5:fa6ff327713c 81 }
stephanie_liu 5:fa6ff327713c 82 } else {
stephanie_liu 5:fa6ff327713c 83 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 84 aciona_motor(tempo, false, motores[i]);
stephanie_liu 5:fa6ff327713c 85 wait_ms(3);
stephanie_liu 5:fa6ff327713c 86 bool bateu = first_read && endstops.read();
stephanie_liu 5:fa6ff327713c 87 if (bateu) {
stephanie_liu 5:fa6ff327713c 88 ref_feito[i] = true;
stephanie_liu 5:fa6ff327713c 89 min_coord[i] = step[i];
stephanie_liu 5:fa6ff327713c 90 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 91 aciona_motor(10, false, motores[i]);
stephanie_liu 5:fa6ff327713c 92 }
stephanie_liu 5:fa6ff327713c 93 }
stephanie_liu 5:fa6ff327713c 94 }
stephanie_liu 5:fa6ff327713c 95 }
stephanie_liu 5:fa6ff327713c 96 }
stephanie_liu 5:fa6ff327713c 97
stephanie_liu 5:fa6ff327713c 98 void Controlador::motor_joystick(int x, int y, bool z1, bool z2) {
stephanie_liu 5:fa6ff327713c 99 //BusOut motores[3] = {motor_x, motor_y, motor_z};
stephanie_liu 5:fa6ff327713c 100 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 101 if (enable) {
stephanie_liu 5:fa6ff327713c 102 if (x > CXmax && step[0] < max_coord[0]) {
stephanie_liu 5:fa6ff327713c 103 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 104 aciona_motor(tempo, true, motores[0]);
stephanie_liu 5:fa6ff327713c 105 step[0] += 4;
stephanie_liu 5:fa6ff327713c 106 } else if (x < CXmin && step[0] > min_coord[0]) {
stephanie_liu 5:fa6ff327713c 107 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 108 aciona_motor(tempo, false, motores[0]);
stephanie_liu 5:fa6ff327713c 109 step[0] -= 4;
stephanie_liu 5:fa6ff327713c 110 } else {
stephanie_liu 5:fa6ff327713c 111 desliga_motor(motores[0]);
stephanie_liu 5:fa6ff327713c 112 }
stephanie_liu 5:fa6ff327713c 113
stephanie_liu 5:fa6ff327713c 114 if (y > CYmax && step[1] < max_coord[1]) {
stephanie_liu 5:fa6ff327713c 115 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 116 aciona_motor(tempo, true, motores[1]);
stephanie_liu 5:fa6ff327713c 117 step[1] += 4;
stephanie_liu 5:fa6ff327713c 118 } else if (y < CYmin && step[1] > min_coord[1]) {
stephanie_liu 5:fa6ff327713c 119 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 120 aciona_motor(tempo, false, motores[1]);
stephanie_liu 5:fa6ff327713c 121 step[1] -= 4;
stephanie_liu 5:fa6ff327713c 122 } else {
stephanie_liu 5:fa6ff327713c 123 desliga_motor(motores[1]);
stephanie_liu 5:fa6ff327713c 124 }
stephanie_liu 5:fa6ff327713c 125
stephanie_liu 5:fa6ff327713c 126 if (z1 && step[2] < max_coord[2]) {
stephanie_liu 5:fa6ff327713c 127 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 128 aciona_motor(tempo, true, motores[2]);
stephanie_liu 5:fa6ff327713c 129 step[2] += 4;
stephanie_liu 5:fa6ff327713c 130 } else if (z2 && step[2] > min_coord[2]) {
stephanie_liu 5:fa6ff327713c 131 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 132 aciona_motor(tempo, false, motores[2]);
stephanie_liu 5:fa6ff327713c 133 step[2] -= 4;
stephanie_liu 5:fa6ff327713c 134 } else {
stephanie_liu 5:fa6ff327713c 135 desliga_motor(motores[2]);
stephanie_liu 5:fa6ff327713c 136 }
stephanie_liu 5:fa6ff327713c 137 display();
stephanie_liu 5:fa6ff327713c 138 }
stephanie_liu 5:fa6ff327713c 139 }
stephanie_liu 5:fa6ff327713c 140
stephanie_liu 5:fa6ff327713c 141 void Controlador::ponto_coleta() {
stephanie_liu 5:fa6ff327713c 142 //pc.printf("determinando coleta\r\n");
stephanie_liu 5:fa6ff327713c 143 coleta[0] = step[0];
stephanie_liu 5:fa6ff327713c 144 coleta[1] = step[1];
stephanie_liu 5:fa6ff327713c 145 coleta[2] = step[2];
stephanie_liu 5:fa6ff327713c 146 }
stephanie_liu 5:fa6ff327713c 147
stephanie_liu 5:fa6ff327713c 148 void Controlador::ponto_solta(int volume_desejado) {
stephanie_liu 5:fa6ff327713c 149 //pc.printf("determinando solta\r\n");
stephanie_liu 5:fa6ff327713c 150 solta[soltas].coord[0] = step[0];
stephanie_liu 5:fa6ff327713c 151 solta[soltas].coord[1] = step[1];
stephanie_liu 5:fa6ff327713c 152 solta[soltas].coord[2] = step[2];
stephanie_liu 5:fa6ff327713c 153 solta[soltas].volume_desejado = volume_desejado;
stephanie_liu 5:fa6ff327713c 154 }
stephanie_liu 5:fa6ff327713c 155
stephanie_liu 5:fa6ff327713c 156 void Controlador::ir_ponto(int destino[3]) {
stephanie_liu 5:fa6ff327713c 157 //BusOut motores[3] = {motor_x, motor_y, motor_z};
stephanie_liu 5:fa6ff327713c 158 // Levantando pipeta no maximo
stephanie_liu 5:fa6ff327713c 159 while (step[2] < max_coord[2]) {
stephanie_liu 5:fa6ff327713c 160 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 161 aciona_motor(tempo, false, motores[2]);
stephanie_liu 5:fa6ff327713c 162 }
stephanie_liu 5:fa6ff327713c 163 // Arrumando eixo x e y
stephanie_liu 5:fa6ff327713c 164 for (int i = 0; i < 2; i++) {
stephanie_liu 5:fa6ff327713c 165 // indo com eixo x e y para SAH
stephanie_liu 5:fa6ff327713c 166 while (step[i] < destino[i]) {
stephanie_liu 5:fa6ff327713c 167 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 168 aciona_motor(tempo, false, motores[i]);
stephanie_liu 5:fa6ff327713c 169 }
stephanie_liu 5:fa6ff327713c 170 // indo com eixo x e y para SH
stephanie_liu 5:fa6ff327713c 171 while (step[i] > destino[i]) {
stephanie_liu 5:fa6ff327713c 172 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 173 aciona_motor(tempo, true, motores[i]);
stephanie_liu 5:fa6ff327713c 174 }
stephanie_liu 5:fa6ff327713c 175 }
stephanie_liu 5:fa6ff327713c 176 // Descendo pipeta para ponto desejado
stephanie_liu 5:fa6ff327713c 177 while (step[2] < destino[2]) {
stephanie_liu 5:fa6ff327713c 178 if (emergencia) return;
stephanie_liu 5:fa6ff327713c 179 aciona_motor(tempo, false, motores[2]);
stephanie_liu 5:fa6ff327713c 180 }
stephanie_liu 5:fa6ff327713c 181 }
stephanie_liu 5:fa6ff327713c 182
stephanie_liu 5:fa6ff327713c 183 void Controlador::coletar() {
stephanie_liu 5:fa6ff327713c 184 ir_ponto(coleta);
stephanie_liu 5:fa6ff327713c 185 led_azul = true;
stephanie_liu 5:fa6ff327713c 186 }
stephanie_liu 5:fa6ff327713c 187
stephanie_liu 5:fa6ff327713c 188 void Controlador::soltar() {
stephanie_liu 5:fa6ff327713c 189 ir_ponto(solta[soltas].coord);
stephanie_liu 5:fa6ff327713c 190 led_azul = false;
stephanie_liu 5:fa6ff327713c 191 solta[soltas].volume_atual++;
stephanie_liu 5:fa6ff327713c 192 if (solta[soltas].volume_atual == solta[soltas].volume_desejado) {
stephanie_liu 5:fa6ff327713c 193 soltas++;
stephanie_liu 5:fa6ff327713c 194 }
stephanie_liu 5:fa6ff327713c 195 }
stephanie_liu 5:fa6ff327713c 196
stephanie_liu 5:fa6ff327713c 197 // 127 - 135 Centro
stephanie_liu 5:fa6ff327713c 198 // >220 Xmax
stephanie_liu 5:fa6ff327713c 199 // >25 Xmin
stephanie_liu 5:fa6ff327713c 200
stephanie_liu 5:fa6ff327713c 201 // 120 - 125 Centro Y
stephanie_liu 5:fa6ff327713c 202 // >210 Ymax
stephanie_liu 5:fa6ff327713c 203 // >25 Ymin