Felipe Telles / Mbed 2 deprecated Bottle_Tracker_Insper

Código final do sistema para movimentar recipientes e escolher pela cor e presença de água

Dependencies:   TCS3200 TextLCD mbed

main.cpp@0:924ec4a5d21b, 2017-06-09 (annotated)

Committer:
felipeto1
Date:
Fri Jun 09 18:04:33 2017 +0000
Revision:
0:924ec4a5d21b
Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
felipeto1 0:924ec4a5d21b 1
felipeto1 0:924ec4a5d21b 2 /* Includes ------------------------------------------------------------------*/
felipeto1 0:924ec4a5d21b 3
felipeto1 0:924ec4a5d21b 4 /* mbed specific header files. */
felipeto1 0:924ec4a5d21b 5 #include "mbed.h"
felipeto1 0:924ec4a5d21b 6 //#include "TCS3200.h"
felipeto1 0:924ec4a5d21b 7
felipeto1 0:924ec4a5d21b 8 #include "TextLCD.h"
felipeto1 0:924ec4a5d21b 9
felipeto1 0:924ec4a5d21b 10
felipeto1 0:924ec4a5d21b 11 TextLCD lcd(PC_13, PB_7, PA_14, PB_12, PB_2, PB_11, TextLCD::LCD20x4); // rs, e, d4-d7
felipeto1 0:924ec4a5d21b 12
felipeto1 0:924ec4a5d21b 13 //lcd.setBacklight(TextLCD::LightOn);
felipeto1 0:924ec4a5d21b 14 //lcd.printf("Billy gordo");
felipeto1 0:924ec4a5d21b 15
felipeto1 0:924ec4a5d21b 16 //#include "TCS3200.h"
felipeto1 0:924ec4a5d21b 17 //TCS3200 color(PC_4, PB_13, PB_14, PB_15, PA_15); //Create a TCS3200 object
felipeto1 0:924ec4a5d21b 18 // S0 S1 S2 S3 OUT
felipeto1 0:924ec4a5d21b 19
felipeto1 0:924ec4a5d21b 20
felipeto1 0:924ec4a5d21b 21 /*------------------------- Implementação LCD --------------------------------*/
felipeto1 0:924ec4a5d21b 22
felipeto1 0:924ec4a5d21b 23 //#include "TextLCD.h"
felipeto1 0:924ec4a5d21b 24
felipeto1 0:924ec4a5d21b 25 // Host PC Communication channels
felipeto1 0:924ec4a5d21b 26 //Serial pc(USBTX, USBRX); // tx, rx
felipeto1 0:924ec4a5d21b 27
felipeto1 0:924ec4a5d21b 28 // I2C Communication
felipeto1 0:924ec4a5d21b 29 //I2C i2c_lcd(D14,D15); // SDA, SCL
felipeto1 0:924ec4a5d21b 30
felipeto1 0:924ec4a5d21b 31 //TextLCD_I2C lcd(&i2c_lcd, 0x7E, TextLCD::LCD20x4);
felipeto1 0:924ec4a5d21b 32
felipeto1 0:924ec4a5d21b 33
felipeto1 0:924ec4a5d21b 34 /* Definitions ---------------------------------------------------------------*/
felipeto1 0:924ec4a5d21b 35
felipeto1 0:924ec4a5d21b 36
felipeto1 0:924ec4a5d21b 37
felipeto1 0:924ec4a5d21b 38 /* Variables -----------------------------------------------------------------*/
felipeto1 0:924ec4a5d21b 39 //InterruptIn Botao_X(D11);
felipeto1 0:924ec4a5d21b 40 //InterruptIn Botao_Y(D12);
felipeto1 0:924ec4a5d21b 41 //InterruptIn Botao_Z(D13);
felipeto1 0:924ec4a5d21b 42 //InterruptIn Botao_Ref(PC_12);
felipeto1 0:924ec4a5d21b 43 //InterruptIn Botao_Ref_Go(PC_8);
felipeto1 0:924ec4a5d21b 44 //InterruptIn Botao_positivo(PC_2);
felipeto1 0:924ec4a5d21b 45 InterruptIn FDCX1(PC_9);
felipeto1 0:924ec4a5d21b 46 InterruptIn FDCX2(PB_9);\
felipeto1 0:924ec4a5d21b 47 InterruptIn FDCY1(PB_9);
felipeto1 0:924ec4a5d21b 48 InterruptIn FDCY2(PB_6);
felipeto1 0:924ec4a5d21b 49 InterruptIn FDCZ1(PA_7);
felipeto1 0:924ec4a5d21b 50 InterruptIn FDCZ2(PC_7);
felipeto1 0:924ec4a5d21b 51 InterruptIn botaox1(PC_3);
felipeto1 0:924ec4a5d21b 52 InterruptIn botaoy1(PC_5);
felipeto1 0:924ec4a5d21b 53 InterruptIn botaoz1(PC_10);
felipeto1 0:924ec4a5d21b 54 InterruptIn botaox2(PA_13);
felipeto1 0:924ec4a5d21b 55 InterruptIn botaoy2(PC_2);
felipeto1 0:924ec4a5d21b 56 InterruptIn botaoz2(PC_12);
felipeto1 0:924ec4a5d21b 57 InterruptIn STOP(PA_11);
felipeto1 0:924ec4a5d21b 58 InterruptIn HANDL(PC_8);
felipeto1 0:924ec4a5d21b 59 //InterruptIn FDCZ1(PC_2);
felipeto1 0:924ec4a5d21b 60 //InterruptIn FDCZ2(PC_2);
felipeto1 0:924ec4a5d21b 61
felipeto1 0:924ec4a5d21b 62 //ENTRADAS MOTOR
felipeto1 0:924ec4a5d21b 63 DigitalOut Eixo_X_Pulso(D2);
felipeto1 0:924ec4a5d21b 64 DigitalOut Eixo_Y_Pulso(D3);
felipeto1 0:924ec4a5d21b 65 DigitalOut Eixo_Z_Pulso(D4);
felipeto1 0:924ec4a5d21b 66 DigitalOut Eixo_X_Dir(D5);
felipeto1 0:924ec4a5d21b 67 DigitalOut Eixo_Y_Dir(D6);
felipeto1 0:924ec4a5d21b 68 DigitalOut Eixo_Z_Dir(D7);
felipeto1 0:924ec4a5d21b 69 DigitalOut Enable(D8);
felipeto1 0:924ec4a5d21b 70 DigitalOut Valv(PB_0);
felipeto1 0:924ec4a5d21b 71 DigitalOut LEDSTART(PA_4);
felipeto1 0:924ec4a5d21b 72 DigitalOut LEDSTOP(PA_1);
felipeto1 0:924ec4a5d21b 73 DigitalOut LEDHANDLE(PA_0);
felipeto1 0:924ec4a5d21b 74
felipeto1 0:924ec4a5d21b 75 //ENTRADAS MOTOR
felipeto1 0:924ec4a5d21b 76
felipeto1 0:924ec4a5d21b 77 DigitalIn Capac(PC_1);
felipeto1 0:924ec4a5d21b 78 DigitalIn START(PC_0);
felipeto1 0:924ec4a5d21b 79
felipeto1 0:924ec4a5d21b 80
felipeto1 0:924ec4a5d21b 81 //DigitalIn Botao_Y_positivo(A0);
felipeto1 0:924ec4a5d21b 82 //DigitalIn Botao_Y_negativo(A1);
felipeto1 0:924ec4a5d21b 83
felipeto1 0:924ec4a5d21b 84 //DigitalIn Botao_Z_positivo(A0);
felipeto1 0:924ec4a5d21b 85 //DigitalIn Botao_Z_negativo(A1);
felipeto1 0:924ec4a5d21b 86
felipeto1 0:924ec4a5d21b 87 //InterruptIn Botao_FIM_X_MAX(A4);
felipeto1 0:924ec4a5d21b 88 //InterruptIn Botao_FIM_X_MIN(A5);
felipeto1 0:924ec4a5d21b 89
felipeto1 0:924ec4a5d21b 90 //DigitalIn Botao_FIM_X_MAX(A3);
felipeto1 0:924ec4a5d21b 91 //DigitalIn Botao_FIM_X_MIN(A4);
felipeto1 0:924ec4a5d21b 92
felipeto1 0:924ec4a5d21b 93 //DigitalIn Botao_FIM_X_MAX(A3);
felipeto1 0:924ec4a5d21b 94 //DigitalIn Botao_FIM_X_MIN(A4);
felipeto1 0:924ec4a5d21b 95
felipeto1 0:924ec4a5d21b 96 int pos_gar_x = 0;
felipeto1 0:924ec4a5d21b 97 int pos_gar_y = 0;
felipeto1 0:924ec4a5d21b 98 int pos_gar_z = 0;
felipeto1 0:924ec4a5d21b 99 int solta_x1 = 0;
felipeto1 0:924ec4a5d21b 100 int solta_x2 = 0;
felipeto1 0:924ec4a5d21b 101 int solta_x3 = 0;
felipeto1 0:924ec4a5d21b 102 int solta_y1 = 0;
felipeto1 0:924ec4a5d21b 103 int solta_y2 = 0;
felipeto1 0:924ec4a5d21b 104 int solta_y3 = 0;
felipeto1 0:924ec4a5d21b 105 int solta_z1 = 0;
felipeto1 0:924ec4a5d21b 106 int solta_z2 = 0;
felipeto1 0:924ec4a5d21b 107 int solta_z3 = 0;
felipeto1 0:924ec4a5d21b 108 int pos_x1 = 0;
felipeto1 0:924ec4a5d21b 109 int pos_x2 = 0;
felipeto1 0:924ec4a5d21b 110 int pos_x3 = 0;
felipeto1 0:924ec4a5d21b 111 int pos_y1 = 0;
felipeto1 0:924ec4a5d21b 112 int pos_y2 = 0;
felipeto1 0:924ec4a5d21b 113 int pos_y3 = 0;
felipeto1 0:924ec4a5d21b 114 int pos_z1 = 0;
felipeto1 0:924ec4a5d21b 115 int pos_z2= 0;
felipeto1 0:924ec4a5d21b 116 int pos_z3 = 0;
felipeto1 0:924ec4a5d21b 117 int zera = 0;
felipeto1 0:924ec4a5d21b 118 int HANDLE = 0;
felipeto1 0:924ec4a5d21b 119 int STARTCODE = 0;
felipeto1 0:924ec4a5d21b 120 int posmidx = 0;
felipeto1 0:924ec4a5d21b 121 int posmidy = 0;
felipeto1 0:924ec4a5d21b 122 int posmidz = 0;
felipeto1 0:924ec4a5d21b 123
felipeto1 0:924ec4a5d21b 124 //var do motor
felipeto1 0:924ec4a5d21b 125 bool dir = false;
felipeto1 0:924ec4a5d21b 126 int cont_pulses = 0;
felipeto1 0:924ec4a5d21b 127 int cur_pos_x = 0;
felipeto1 0:924ec4a5d21b 128 int cur_pos_y = 0;
felipeto1 0:924ec4a5d21b 129 int cur_pos_z = 0;
felipeto1 0:924ec4a5d21b 130 float velocidade = 0.00032;//de 0.01 a 0.0004
felipeto1 0:924ec4a5d21b 131 float velocidade2 = 0.0005;
felipeto1 0:924ec4a5d21b 132 bool auto_rot = false;
felipeto1 0:924ec4a5d21b 133 //var do motor
felipeto1 0:924ec4a5d21b 134
felipeto1 0:924ec4a5d21b 135
felipeto1 0:924ec4a5d21b 136
felipeto1 0:924ec4a5d21b 137 /*------------------------ Funções --------------------------*/
felipeto1 0:924ec4a5d21b 138
felipeto1 0:924ec4a5d21b 139 void ligabobina(){
felipeto1 0:924ec4a5d21b 140 Valv = 1;
felipeto1 0:924ec4a5d21b 141 }
felipeto1 0:924ec4a5d21b 142
felipeto1 0:924ec4a5d21b 143 void desligabobina(){
felipeto1 0:924ec4a5d21b 144 Valv = 0;
felipeto1 0:924ec4a5d21b 145 }
felipeto1 0:924ec4a5d21b 146 /*
felipeto1 0:924ec4a5d21b 147 int rgb() { //--------------------------------------PERFEITO
felipeto1 0:924ec4a5d21b 148 int variavel;
felipeto1 0:924ec4a5d21b 149 long red, green, blue, clear;
felipeto1 0:924ec4a5d21b 150 color.SetMode(TCS3200::SCALE_20);
felipeto1 0:924ec4a5d21b 151 while(x<100){
felipeto1 0:924ec4a5d21b 152 red = color.ReadRed();
felipeto1 0:924ec4a5d21b 153 green = color.ReadGreen();
felipeto1 0:924ec4a5d21b 154 blue = color.ReadBlue();
felipeto1 0:924ec4a5d21b 155 clear = color.ReadClear();
felipeto1 0:924ec4a5d21b 156 //printf("RED: %10d GREEN: %10d BLUE: %10d CLEAR: %10d \r\n ", red, green, blue, clear);
felipeto1 0:924ec4a5d21b 157 if((red<green)&&(red<blue)){
felipeto1 0:924ec4a5d21b 158 //printf("Color: Red \r\n");
felipeto1 0:924ec4a5d21b 159 variavel=1
felipeto1 0:924ec4a5d21b 160 }
felipeto1 0:924ec4a5d21b 161 if((green<red)&&(green<blue)){
felipeto1 0:924ec4a5d21b 162 //printf("Color: Green \r\n");
felipeto1 0:924ec4a5d21b 163 }
felipeto1 0:924ec4a5d21b 164 if((blue<red)&&(blue<green)){
felipeto1 0:924ec4a5d21b 165 //printf("Color: Blue \r\n");
felipeto1 0:924ec4a5d21b 166 }
felipeto1 0:924ec4a5d21b 167 variavel = 0;
felipeto1 0:924ec4a5d21b 168 wait(0.1);
felipeto1 0:924ec4a5d21b 169 x++;
felipeto1 0:924ec4a5d21b 170 }
felipeto1 0:924ec4a5d21b 171 return variavel;
felipeto1 0:924ec4a5d21b 172 }
felipeto1 0:924ec4a5d21b 173 */
felipeto1 0:924ec4a5d21b 174 /*
felipeto1 0:924ec4a5d21b 175 int rgb(){
felipeto1 0:924ec4a5d21b 176 long red, green, blue, clear;
felipeto1 0:924ec4a5d21b 177
felipeto1 0:924ec4a5d21b 178 //Set the scaling factor to 100%
felipeto1 0:924ec4a5d21b 179 color.SetMode(TCS3200::SCALE_100);
felipeto1 0:924ec4a5d21b 180 int bob=0;
felipeto1 0:924ec4a5d21b 181 int cor=0;
felipeto1 0:924ec4a5d21b 182 while(bob<10){
felipeto1 0:924ec4a5d21b 183 //Read the HIGH pulse width in nS for each color.
felipeto1 0:924ec4a5d21b 184 //The lower the value, the more of that color is detected
felipeto1 0:924ec4a5d21b 185 red = color.ReadRed();
felipeto1 0:924ec4a5d21b 186 green = color.ReadGreen();
felipeto1 0:924ec4a5d21b 187 blue = color.ReadBlue();
felipeto1 0:924ec4a5d21b 188 clear = color.ReadClear();
felipeto1 0:924ec4a5d21b 189
felipeto1 0:924ec4a5d21b 190 printf("RED: %10d GREEN: %10d BLUE: %10d CLEAR: %10d ", red, green, blue, clear);
felipeto1 0:924ec4a5d21b 191
felipeto1 0:924ec4a5d21b 192 wait(0.1);
felipeto1 0:924ec4a5d21b 193
felipeto1 0:924ec4a5d21b 194 if(red>50)
felipeto1 0:924ec4a5d21b 195 {
felipeto1 0:924ec4a5d21b 196 printf("VERMELHO. \r\n"); // Vermelho
felipeto1 0:924ec4a5d21b 197 cor=1;
felipeto1 0:924ec4a5d21b 198 }
felipeto1 0:924ec4a5d21b 199 if(blue>50)
felipeto1 0:924ec4a5d21b 200 {
felipeto1 0:924ec4a5d21b 201 printf("AZUL. \r\n"); // Azul
felipeto1 0:924ec4a5d21b 202 cor=0;
felipeto1 0:924ec4a5d21b 203 }
felipeto1 0:924ec4a5d21b 204
felipeto1 0:924ec4a5d21b 205 else
felipeto1 0:924ec4a5d21b 206 {
felipeto1 0:924ec4a5d21b 207 printf("VERDE. \r\n"); // Verde
felipeto1 0:924ec4a5d21b 208 }
felipeto1 0:924ec4a5d21b 209 bob++;
felipeto1 0:924ec4a5d21b 210 }
felipeto1 0:924ec4a5d21b 211
felipeto1 0:924ec4a5d21b 212 return cor;
felipeto1 0:924ec4a5d21b 213 }
felipeto1 0:924ec4a5d21b 214 */
felipeto1 0:924ec4a5d21b 215 int rgb(){
felipeto1 0:924ec4a5d21b 216 return;
felipeto1 0:924ec4a5d21b 217 }
felipeto1 0:924ec4a5d21b 218
felipeto1 0:924ec4a5d21b 219 int capacitivo(void){
felipeto1 0:924ec4a5d21b 220 int y=0;
felipeto1 0:924ec4a5d21b 221 int C=0;
felipeto1 0:924ec4a5d21b 222 while (y<10){
felipeto1 0:924ec4a5d21b 223 int C= Capac;
felipeto1 0:924ec4a5d21b 224 y++;
felipeto1 0:924ec4a5d21b 225 }
felipeto1 0:924ec4a5d21b 226 return C;
felipeto1 0:924ec4a5d21b 227 }
felipeto1 0:924ec4a5d21b 228
felipeto1 0:924ec4a5d21b 229 void direcao(int motor,int posf){
felipeto1 0:924ec4a5d21b 230 dir = true ;
felipeto1 0:924ec4a5d21b 231 switch(motor){
felipeto1 0:924ec4a5d21b 232 case 1:
felipeto1 0:924ec4a5d21b 233 if (posf<cur_pos_x){
felipeto1 0:924ec4a5d21b 234 while(dir == true && cur_pos_x !=posf){
felipeto1 0:924ec4a5d21b 235 Eixo_X_Dir = 0;
felipeto1 0:924ec4a5d21b 236 Eixo_X_Pulso = 1;
felipeto1 0:924ec4a5d21b 237 wait(velocidade);
felipeto1 0:924ec4a5d21b 238 Eixo_X_Pulso = 0;
felipeto1 0:924ec4a5d21b 239 wait(velocidade);
felipeto1 0:924ec4a5d21b 240 cur_pos_x -= 1;
felipeto1 0:924ec4a5d21b 241 //dir == false;
felipeto1 0:924ec4a5d21b 242 }}
felipeto1 0:924ec4a5d21b 243
felipeto1 0:924ec4a5d21b 244 if (posf>cur_pos_x){
felipeto1 0:924ec4a5d21b 245 while(dir == true && cur_pos_x !=posf){
felipeto1 0:924ec4a5d21b 246 Eixo_X_Dir = 1;
felipeto1 0:924ec4a5d21b 247 Eixo_X_Pulso = 1;
felipeto1 0:924ec4a5d21b 248 wait(velocidade);
felipeto1 0:924ec4a5d21b 249 Eixo_X_Pulso = 0;
felipeto1 0:924ec4a5d21b 250 wait(velocidade);
felipeto1 0:924ec4a5d21b 251 cur_pos_x += 1;
felipeto1 0:924ec4a5d21b 252 //dir == false;
felipeto1 0:924ec4a5d21b 253 }}
felipeto1 0:924ec4a5d21b 254 break;
felipeto1 0:924ec4a5d21b 255
felipeto1 0:924ec4a5d21b 256 case 2:
felipeto1 0:924ec4a5d21b 257 if (posf<cur_pos_y){
felipeto1 0:924ec4a5d21b 258 while(dir==true && cur_pos_y !=posf){
felipeto1 0:924ec4a5d21b 259 Eixo_Y_Dir = 0;
felipeto1 0:924ec4a5d21b 260 Eixo_Y_Pulso = 1;
felipeto1 0:924ec4a5d21b 261 wait(velocidade);
felipeto1 0:924ec4a5d21b 262 Eixo_Y_Pulso = 0;
felipeto1 0:924ec4a5d21b 263 wait(velocidade);
felipeto1 0:924ec4a5d21b 264 cur_pos_y -= 1;
felipeto1 0:924ec4a5d21b 265 //dir == false;
felipeto1 0:924ec4a5d21b 266 }}
felipeto1 0:924ec4a5d21b 267
felipeto1 0:924ec4a5d21b 268 if (posf>cur_pos_y){
felipeto1 0:924ec4a5d21b 269 while(dir==true && cur_pos_y !=posf){
felipeto1 0:924ec4a5d21b 270 Eixo_Y_Dir = 1;
felipeto1 0:924ec4a5d21b 271 Eixo_Y_Pulso = 1;
felipeto1 0:924ec4a5d21b 272 wait(velocidade);
felipeto1 0:924ec4a5d21b 273 Eixo_Y_Pulso = 0;
felipeto1 0:924ec4a5d21b 274 wait(velocidade);
felipeto1 0:924ec4a5d21b 275 cur_pos_y += 1;
felipeto1 0:924ec4a5d21b 276 //dir == false;
felipeto1 0:924ec4a5d21b 277 }}
felipeto1 0:924ec4a5d21b 278
felipeto1 0:924ec4a5d21b 279 break;
felipeto1 0:924ec4a5d21b 280
felipeto1 0:924ec4a5d21b 281 case 3:
felipeto1 0:924ec4a5d21b 282 if (posf<cur_pos_z){
felipeto1 0:924ec4a5d21b 283 while(dir == true && cur_pos_z !=posf){
felipeto1 0:924ec4a5d21b 284 Eixo_Z_Dir = 0;
felipeto1 0:924ec4a5d21b 285 Eixo_Z_Pulso = 1;
felipeto1 0:924ec4a5d21b 286 wait(velocidade);
felipeto1 0:924ec4a5d21b 287 Eixo_Z_Pulso = 0;
felipeto1 0:924ec4a5d21b 288 wait(velocidade);
felipeto1 0:924ec4a5d21b 289 cur_pos_z -= 1;
felipeto1 0:924ec4a5d21b 290 //dir == false;
felipeto1 0:924ec4a5d21b 291 }
felipeto1 0:924ec4a5d21b 292 }
felipeto1 0:924ec4a5d21b 293 if (posf>cur_pos_z){
felipeto1 0:924ec4a5d21b 294 while(dir == true && cur_pos_z !=posf){
felipeto1 0:924ec4a5d21b 295 Eixo_Z_Dir = 1;
felipeto1 0:924ec4a5d21b 296 Eixo_Z_Pulso = 1;
felipeto1 0:924ec4a5d21b 297 wait(velocidade);
felipeto1 0:924ec4a5d21b 298 Eixo_Z_Pulso = 0;
felipeto1 0:924ec4a5d21b 299 wait(velocidade);
felipeto1 0:924ec4a5d21b 300 cur_pos_z += 1;
felipeto1 0:924ec4a5d21b 301 //dir == false;
felipeto1 0:924ec4a5d21b 302 }
felipeto1 0:924ec4a5d21b 303 }
felipeto1 0:924ec4a5d21b 304 break;
felipeto1 0:924ec4a5d21b 305
felipeto1 0:924ec4a5d21b 306 }
felipeto1 0:924ec4a5d21b 307 return;
felipeto1 0:924ec4a5d21b 308 }
felipeto1 0:924ec4a5d21b 309
felipeto1 0:924ec4a5d21b 310
felipeto1 0:924ec4a5d21b 311 void direcao_p(int motor){
felipeto1 0:924ec4a5d21b 312 dir = true ;
felipeto1 0:924ec4a5d21b 313 switch(motor){
felipeto1 0:924ec4a5d21b 314 while(dir == true){
felipeto1 0:924ec4a5d21b 315 case 1:
felipeto1 0:924ec4a5d21b 316 Eixo_X_Dir = 1;
felipeto1 0:924ec4a5d21b 317 Eixo_X_Pulso = 1;
felipeto1 0:924ec4a5d21b 318 wait(velocidade);
felipeto1 0:924ec4a5d21b 319 Eixo_X_Pulso = 0;
felipeto1 0:924ec4a5d21b 320 wait(velocidade);
felipeto1 0:924ec4a5d21b 321 cur_pos_x += 1;
felipeto1 0:924ec4a5d21b 322 //dir == false;
felipeto1 0:924ec4a5d21b 323 }
felipeto1 0:924ec4a5d21b 324 break;
felipeto1 0:924ec4a5d21b 325
felipeto1 0:924ec4a5d21b 326
felipeto1 0:924ec4a5d21b 327 case 2:
felipeto1 0:924ec4a5d21b 328 while(dir==true){
felipeto1 0:924ec4a5d21b 329 Eixo_Y_Dir = 1;
felipeto1 0:924ec4a5d21b 330 Eixo_Y_Pulso = 1;
felipeto1 0:924ec4a5d21b 331 wait(velocidade);
felipeto1 0:924ec4a5d21b 332 Eixo_Y_Pulso = 0;
felipeto1 0:924ec4a5d21b 333 wait(velocidade);
felipeto1 0:924ec4a5d21b 334 cur_pos_y += 1;
felipeto1 0:924ec4a5d21b 335 //dir == false;
felipeto1 0:924ec4a5d21b 336 }
felipeto1 0:924ec4a5d21b 337 break;
felipeto1 0:924ec4a5d21b 338
felipeto1 0:924ec4a5d21b 339
felipeto1 0:924ec4a5d21b 340 case 3:
felipeto1 0:924ec4a5d21b 341 while(dir == true){
felipeto1 0:924ec4a5d21b 342 Eixo_Z_Dir = 1;
felipeto1 0:924ec4a5d21b 343 Eixo_Z_Pulso = 1;
felipeto1 0:924ec4a5d21b 344 wait(velocidade);
felipeto1 0:924ec4a5d21b 345 Eixo_Z_Pulso = 0;
felipeto1 0:924ec4a5d21b 346 wait(velocidade);
felipeto1 0:924ec4a5d21b 347 cur_pos_z += 1;
felipeto1 0:924ec4a5d21b 348 //dir == false;
felipeto1 0:924ec4a5d21b 349 }
felipeto1 0:924ec4a5d21b 350 break;
felipeto1 0:924ec4a5d21b 351
felipeto1 0:924ec4a5d21b 352
felipeto1 0:924ec4a5d21b 353
felipeto1 0:924ec4a5d21b 354 }
felipeto1 0:924ec4a5d21b 355 return;
felipeto1 0:924ec4a5d21b 356 }
felipeto1 0:924ec4a5d21b 357
felipeto1 0:924ec4a5d21b 358 void FDCY(){
felipeto1 0:924ec4a5d21b 359 cur_pos_y=0;
felipeto1 0:924ec4a5d21b 360 direcao(2,1500);
felipeto1 0:924ec4a5d21b 361 cur_pos_y=0;
felipeto1 0:924ec4a5d21b 362 }
felipeto1 0:924ec4a5d21b 363
felipeto1 0:924ec4a5d21b 364 void motor_off(){
felipeto1 0:924ec4a5d21b 365 dir = false;
felipeto1 0:924ec4a5d21b 366 zera++;
felipeto1 0:924ec4a5d21b 367 }
felipeto1 0:924ec4a5d21b 368
felipeto1 0:924ec4a5d21b 369 void direcao_n(int motor){
felipeto1 0:924ec4a5d21b 370 dir = true ;
felipeto1 0:924ec4a5d21b 371 switch(motor){
felipeto1 0:924ec4a5d21b 372 while(dir == true){
felipeto1 0:924ec4a5d21b 373 case 1:
felipeto1 0:924ec4a5d21b 374 Eixo_X_Dir = 0;
felipeto1 0:924ec4a5d21b 375 Eixo_X_Pulso = 1;
felipeto1 0:924ec4a5d21b 376 wait(velocidade);
felipeto1 0:924ec4a5d21b 377 Eixo_X_Pulso = 0;
felipeto1 0:924ec4a5d21b 378 wait(velocidade);
felipeto1 0:924ec4a5d21b 379 cur_pos_x -= 1;
felipeto1 0:924ec4a5d21b 380 //dir == false;
felipeto1 0:924ec4a5d21b 381 }
felipeto1 0:924ec4a5d21b 382 break;
felipeto1 0:924ec4a5d21b 383
felipeto1 0:924ec4a5d21b 384
felipeto1 0:924ec4a5d21b 385 case 2:
felipeto1 0:924ec4a5d21b 386 while(dir==true){
felipeto1 0:924ec4a5d21b 387 Eixo_Y_Dir = 0;
felipeto1 0:924ec4a5d21b 388 Eixo_Y_Pulso = 1;
felipeto1 0:924ec4a5d21b 389 wait(velocidade);
felipeto1 0:924ec4a5d21b 390 Eixo_Y_Pulso = 0;
felipeto1 0:924ec4a5d21b 391 wait(velocidade);
felipeto1 0:924ec4a5d21b 392 cur_pos_y -= 1;
felipeto1 0:924ec4a5d21b 393 //dir == false;
felipeto1 0:924ec4a5d21b 394 }
felipeto1 0:924ec4a5d21b 395 break;
felipeto1 0:924ec4a5d21b 396
felipeto1 0:924ec4a5d21b 397
felipeto1 0:924ec4a5d21b 398 case 3:
felipeto1 0:924ec4a5d21b 399 while(dir == true){
felipeto1 0:924ec4a5d21b 400 Eixo_Z_Dir = 0;
felipeto1 0:924ec4a5d21b 401 Eixo_Z_Pulso = 1;
felipeto1 0:924ec4a5d21b 402 wait(velocidade);
felipeto1 0:924ec4a5d21b 403 Eixo_Z_Pulso = 0;
felipeto1 0:924ec4a5d21b 404 wait(velocidade);
felipeto1 0:924ec4a5d21b 405 cur_pos_z -= 1;
felipeto1 0:924ec4a5d21b 406 //dir == false;
felipeto1 0:924ec4a5d21b 407 }
felipeto1 0:924ec4a5d21b 408 break;
felipeto1 0:924ec4a5d21b 409
felipeto1 0:924ec4a5d21b 410 }
felipeto1 0:924ec4a5d21b 411 Eixo_X_Pulso = 0;
felipeto1 0:924ec4a5d21b 412 Eixo_Y_Pulso = 0;
felipeto1 0:924ec4a5d21b 413 Eixo_Z_Pulso = 0;
felipeto1 0:924ec4a5d21b 414 return;
felipeto1 0:924ec4a5d21b 415 }
felipeto1 0:924ec4a5d21b 416
felipeto1 0:924ec4a5d21b 417 void parax(){
felipeto1 0:924ec4a5d21b 418 direcao_n(1);
felipeto1 0:924ec4a5d21b 419 }
felipeto1 0:924ec4a5d21b 420
felipeto1 0:924ec4a5d21b 421 void paray(){
felipeto1 0:924ec4a5d21b 422 printf("mexeu");
felipeto1 0:924ec4a5d21b 423 direcao_n(2);
felipeto1 0:924ec4a5d21b 424
felipeto1 0:924ec4a5d21b 425 }
felipeto1 0:924ec4a5d21b 426
felipeto1 0:924ec4a5d21b 427 void paraz(){
felipeto1 0:924ec4a5d21b 428 direcao_p(3);
felipeto1 0:924ec4a5d21b 429 }
felipeto1 0:924ec4a5d21b 430
felipeto1 0:924ec4a5d21b 431
felipeto1 0:924ec4a5d21b 432 void zeramentox(){
felipeto1 0:924ec4a5d21b 433 //if (zera==0){
felipeto1 0:924ec4a5d21b 434 printf("ff");
felipeto1 0:924ec4a5d21b 435 lcd.locate(0,0);
felipeto1 0:924ec4a5d21b 436 lcd.printf("Zerando eixo x");
felipeto1 0:924ec4a5d21b 437 direcao_n(1);
felipeto1 0:924ec4a5d21b 438 }
felipeto1 0:924ec4a5d21b 439
felipeto1 0:924ec4a5d21b 440 void zeramentoy(){
felipeto1 0:924ec4a5d21b 441 lcd.locate(0,0);
felipeto1 0:924ec4a5d21b 442 lcd.printf("Zerando eixo y");
felipeto1 0:924ec4a5d21b 443 //if (zera==1){
felipeto1 0:924ec4a5d21b 444 direcao_p(2);
felipeto1 0:924ec4a5d21b 445 }
felipeto1 0:924ec4a5d21b 446
felipeto1 0:924ec4a5d21b 447 void zeramentoz(){
felipeto1 0:924ec4a5d21b 448 lcd.locate(0,0);
felipeto1 0:924ec4a5d21b 449 lcd.printf("Zerando eixo z");
felipeto1 0:924ec4a5d21b 450 // if (zera==2){
felipeto1 0:924ec4a5d21b 451 direcao_n(3);
felipeto1 0:924ec4a5d21b 452 }
felipeto1 0:924ec4a5d21b 453
felipeto1 0:924ec4a5d21b 454 void handd(){
felipeto1 0:924ec4a5d21b 455 HANDLE=0;
felipeto1 0:924ec4a5d21b 456 LEDHANDLE = 0;
felipeto1 0:924ec4a5d21b 457 }
felipeto1 0:924ec4a5d21b 458
felipeto1 0:924ec4a5d21b 459 void ciclo_off(){
felipeto1 0:924ec4a5d21b 460 dir = false;
felipeto1 0:924ec4a5d21b 461 STARTCODE=0;
felipeto1 0:924ec4a5d21b 462 dir = false;
felipeto1 0:924ec4a5d21b 463
felipeto1 0:924ec4a5d21b 464 }
felipeto1 0:924ec4a5d21b 465
felipeto1 0:924ec4a5d21b 466 void ciclo(int xp, int yp, int zp){
felipeto1 0:924ec4a5d21b 467 direcao(3,posmidz);
felipeto1 0:924ec4a5d21b 468 STARTCODE = 0;
felipeto1 0:924ec4a5d21b 469 LEDSTART = 1;
felipeto1 0:924ec4a5d21b 470 STOP.rise(&ciclo_off);
felipeto1 0:924ec4a5d21b 471 //direcao(1,posmidx);
felipeto1 0:924ec4a5d21b 472 // direcao(2,posmidy);
felipeto1 0:924ec4a5d21b 473 while (1){
felipeto1 0:924ec4a5d21b 474 if (START ==1){
felipeto1 0:924ec4a5d21b 475 STARTCODE=1;
felipeto1 0:924ec4a5d21b 476 }
felipeto1 0:924ec4a5d21b 477 while (STARTCODE == 1){
felipeto1 0:924ec4a5d21b 478 LEDSTOP = 1;
felipeto1 0:924ec4a5d21b 479 direcao(3,zp);
felipeto1 0:924ec4a5d21b 480 ligabobina();
felipeto1 0:924ec4a5d21b 481 direcao(3,posmidz);
felipeto1 0:924ec4a5d21b 482 int ccor = rgb();
felipeto1 0:924ec4a5d21b 483 int capa = capacitivo();
felipeto1 0:924ec4a5d21b 484 lcd.printf("%d",ccor);
felipeto1 0:924ec4a5d21b 485 wait(3);
felipeto1 0:924ec4a5d21b 486 if (ccor==1 && capa == 1 ){
felipeto1 0:924ec4a5d21b 487 direcao(1,solta_x1);
felipeto1 0:924ec4a5d21b 488 direcao(2,solta_y1);
felipeto1 0:924ec4a5d21b 489 direcao(3,zp+100);
felipeto1 0:924ec4a5d21b 490 desligabobina();
felipeto1 0:924ec4a5d21b 491 direcao(3,posmidz);
felipeto1 0:924ec4a5d21b 492 direcao(1,xp);
felipeto1 0:924ec4a5d21b 493 direcao(2,yp);
felipeto1 0:924ec4a5d21b 494
felipeto1 0:924ec4a5d21b 495 }
felipeto1 0:924ec4a5d21b 496 if (ccor == 2 && capa == 1){
felipeto1 0:924ec4a5d21b 497 direcao(1,solta_x2);
felipeto1 0:924ec4a5d21b 498 direcao(2,solta_y2);
felipeto1 0:924ec4a5d21b 499 direcao(3,zp+100);
felipeto1 0:924ec4a5d21b 500 desligabobina();
felipeto1 0:924ec4a5d21b 501 direcao(3,posmidz);
felipeto1 0:924ec4a5d21b 502 direcao(1,xp);
felipeto1 0:924ec4a5d21b 503 direcao(2,yp);
felipeto1 0:924ec4a5d21b 504
felipeto1 0:924ec4a5d21b 505 }
felipeto1 0:924ec4a5d21b 506
felipeto1 0:924ec4a5d21b 507 if (capa == 0){
felipeto1 0:924ec4a5d21b 508 direcao(1,solta_x3);
felipeto1 0:924ec4a5d21b 509 direcao(2,solta_y3);
felipeto1 0:924ec4a5d21b 510 direcao(3,zp+100);
felipeto1 0:924ec4a5d21b 511 desligabobina();
felipeto1 0:924ec4a5d21b 512 direcao(3,posmidz);
felipeto1 0:924ec4a5d21b 513 direcao(1,xp);
felipeto1 0:924ec4a5d21b 514 direcao(2,yp);
felipeto1 0:924ec4a5d21b 515
felipeto1 0:924ec4a5d21b 516 }
felipeto1 0:924ec4a5d21b 517 }
felipeto1 0:924ec4a5d21b 518 //print start
felipeto1 0:924ec4a5d21b 519 }
felipeto1 0:924ec4a5d21b 520 }
felipeto1 0:924ec4a5d21b 521
felipeto1 0:924ec4a5d21b 522
felipeto1 0:924ec4a5d21b 523 void JOG(){
felipeto1 0:924ec4a5d21b 524 lcd.locate(0,0);
felipeto1 0:924ec4a5d21b 525 lcd.printf("JOG ativado");
felipeto1 0:924ec4a5d21b 526 lcd.locate(0,1);
felipeto1 0:924ec4a5d21b 527 lcd.printf("X = %f",cur_pos_x/400);
felipeto1 0:924ec4a5d21b 528 lcd.locate(0,2);
felipeto1 0:924ec4a5d21b 529 lcd.printf("Y = %f",cur_pos_y/400);
felipeto1 0:924ec4a5d21b 530 lcd.locate(0,3);
felipeto1 0:924ec4a5d21b 531 lcd.printf("Z = %f",cur_pos_z/400);
felipeto1 0:924ec4a5d21b 532 HANDL.rise(&handd);
felipeto1 0:924ec4a5d21b 533 HANDLE = 1;
felipeto1 0:924ec4a5d21b 534 LEDHANDLE = 1;
felipeto1 0:924ec4a5d21b 535 //botaox.rise(&parax);
felipeto1 0:924ec4a5d21b 536 botaox1.fall(&motor_off);
felipeto1 0:924ec4a5d21b 537 botaoy1.fall(&motor_off);
felipeto1 0:924ec4a5d21b 538 botaoz1.fall(&motor_off);
felipeto1 0:924ec4a5d21b 539 botaox2.fall(&motor_off);
felipeto1 0:924ec4a5d21b 540 botaoy2.fall(&motor_off);
felipeto1 0:924ec4a5d21b 541 botaoz2.fall(&motor_off);
felipeto1 0:924ec4a5d21b 542 wait(2);
felipeto1 0:924ec4a5d21b 543 while(1){
felipeto1 0:924ec4a5d21b 544 while(HANDLE==1){
felipeto1 0:924ec4a5d21b 545 lcd.cls();
felipeto1 0:924ec4a5d21b 546 lcd.locate(0,0);
felipeto1 0:924ec4a5d21b 547 lcd.printf("JOG ativado");
felipeto1 0:924ec4a5d21b 548 lcd.locate(0,1);
felipeto1 0:924ec4a5d21b 549 float bobaox=3*cur_pos_x/400;
felipeto1 0:924ec4a5d21b 550 float bobaoz=3*cur_pos_z/400;
felipeto1 0:924ec4a5d21b 551 float bobaoy=3*cur_pos_y/400;
felipeto1 0:924ec4a5d21b 552 lcd.printf("X = %.2f mm",bobaox);
felipeto1 0:924ec4a5d21b 553 lcd.locate(0,2);
felipeto1 0:924ec4a5d21b 554 lcd.printf("Y = %.2f mm",bobaoy);
felipeto1 0:924ec4a5d21b 555 lcd.locate(0,3);
felipeto1 0:924ec4a5d21b 556 lcd.printf("Z = %.2f mm",bobaoz);
felipeto1 0:924ec4a5d21b 557 if (botaox1==1){
felipeto1 0:924ec4a5d21b 558 direcao_n(1);
felipeto1 0:924ec4a5d21b 559 }
felipeto1 0:924ec4a5d21b 560 if (botaoy1 == 1){
felipeto1 0:924ec4a5d21b 561 direcao_n(2);
felipeto1 0:924ec4a5d21b 562 }
felipeto1 0:924ec4a5d21b 563 if (botaoz1 == 1){
felipeto1 0:924ec4a5d21b 564 direcao_n(3);
felipeto1 0:924ec4a5d21b 565 }
felipeto1 0:924ec4a5d21b 566 if (botaox2==1){
felipeto1 0:924ec4a5d21b 567 direcao_p(1);
felipeto1 0:924ec4a5d21b 568 }
felipeto1 0:924ec4a5d21b 569 if (botaoy2 == 1){
felipeto1 0:924ec4a5d21b 570 direcao_p(2);
felipeto1 0:924ec4a5d21b 571 }
felipeto1 0:924ec4a5d21b 572 if (botaoz2 == 1){
felipeto1 0:924ec4a5d21b 573 direcao_p(3);
felipeto1 0:924ec4a5d21b 574 }
felipeto1 0:924ec4a5d21b 575 wait(0.2);
felipeto1 0:924ec4a5d21b 576 }
felipeto1 0:924ec4a5d21b 577
felipeto1 0:924ec4a5d21b 578 if (HANDLE == 0){
felipeto1 0:924ec4a5d21b 579 lcd.cls();
felipeto1 0:924ec4a5d21b 580 lcd.printf("Pos PEGAR Salva!");
felipeto1 0:924ec4a5d21b 581 pos_gar_x=cur_pos_x;
felipeto1 0:924ec4a5d21b 582 pos_gar_y=cur_pos_y;
felipeto1 0:924ec4a5d21b 583 pos_gar_z=cur_pos_z;
felipeto1 0:924ec4a5d21b 584 wait(2);
felipeto1 0:924ec4a5d21b 585 ciclo(pos_gar_x,pos_gar_y,pos_gar_z);
felipeto1 0:924ec4a5d21b 586 break;
felipeto1 0:924ec4a5d21b 587 }
felipeto1 0:924ec4a5d21b 588 printf("nada");
felipeto1 0:924ec4a5d21b 589 }}
felipeto1 0:924ec4a5d21b 590
felipeto1 0:924ec4a5d21b 591
felipeto1 0:924ec4a5d21b 592
felipeto1 0:924ec4a5d21b 593 int main(){
felipeto1 0:924ec4a5d21b 594 //lcd.setBacklight(TextLCD::LightOn);
felipeto1 0:924ec4a5d21b 595 lcd.printf("Zeramento em 5");
felipeto1 0:924ec4a5d21b 596 wait(1);
felipeto1 0:924ec4a5d21b 597 lcd.cls();
felipeto1 0:924ec4a5d21b 598 lcd.printf("Zeramento em 4");
felipeto1 0:924ec4a5d21b 599 wait(1);
felipeto1 0:924ec4a5d21b 600 lcd.cls();
felipeto1 0:924ec4a5d21b 601 lcd.printf("Zeramento em 3");
felipeto1 0:924ec4a5d21b 602 wait(1);
felipeto1 0:924ec4a5d21b 603 lcd.cls();
felipeto1 0:924ec4a5d21b 604 lcd.printf("Zeramento em 2");
felipeto1 0:924ec4a5d21b 605 wait(1);
felipeto1 0:924ec4a5d21b 606 lcd.cls();
felipeto1 0:924ec4a5d21b 607 lcd.printf("Zeramento em 1");
felipeto1 0:924ec4a5d21b 608 wait(1);
felipeto1 0:924ec4a5d21b 609 lcd.cls();
felipeto1 0:924ec4a5d21b 610 lcd.printf("Zerando");
felipeto1 0:924ec4a5d21b 611 FDCY1.rise(&motor_off);
felipeto1 0:924ec4a5d21b 612 FDCX1.rise(&motor_off);
felipeto1 0:924ec4a5d21b 613 FDCZ1.rise(&motor_off);
felipeto1 0:924ec4a5d21b 614 zeramentox();
felipeto1 0:924ec4a5d21b 615 cur_pos_x=0;
felipeto1 0:924ec4a5d21b 616 lcd.locate(0,1);
felipeto1 0:924ec4a5d21b 617 lcd.printf("PosX Salva!");
felipeto1 0:924ec4a5d21b 618 wait(1);
felipeto1 0:924ec4a5d21b 619 zeramentoy();
felipeto1 0:924ec4a5d21b 620 cur_pos_y=0;
felipeto1 0:924ec4a5d21b 621 lcd.locate(0,2);
felipeto1 0:924ec4a5d21b 622 lcd.printf("PosY Salva!");
felipeto1 0:924ec4a5d21b 623 wait(1);
felipeto1 0:924ec4a5d21b 624 zeramentoz();//ok
felipeto1 0:924ec4a5d21b 625 lcd.locate(0,3);
felipeto1 0:924ec4a5d21b 626 lcd.printf("PosZ Salva!");
felipeto1 0:924ec4a5d21b 627 cur_pos_z=0;
felipeto1 0:924ec4a5d21b 628 wait(1);
felipeto1 0:924ec4a5d21b 629 JOG();
felipeto1 0:924ec4a5d21b 630 printf("saiu");
felipeto1 0:924ec4a5d21b 631 //ciclo(pos_gar_x,pos_gar_y,pos_gar_z);
felipeto1 0:924ec4a5d21b 632
felipeto1 0:924ec4a5d21b 633 }
felipeto1 0:924ec4a5d21b 634
felipeto1 0:924ec4a5d21b 635