Program version 1
Dependencies: FatFileSystemSD MSCFileSystem TextLCD mbed-rtos mbed
Fork of USBFlashDiskTest by
main.cpp@3:10da2a723362, 2012-11-21 (annotated)
- Committer:
- carlos_nascimento08
- Date:
- Wed Nov 21 17:20:27 2012 +0000
- Revision:
- 3:10da2a723362
- Parent:
- 2:f1f6b33cd7bd
Problem
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chris | 0:22a5db2c7926 | 1 | #include "mbed.h" |
carlos_nascimento08 | 2:f1f6b33cd7bd | 2 | #include "RoboClaw.h" |
carlos_nascimento08 | 2:f1f6b33cd7bd | 3 | #include "TextLCD.h" |
chris | 0:22a5db2c7926 | 4 | #include "MSCFileSystem.h" |
carlos_nascimento08 | 2:f1f6b33cd7bd | 5 | #include "cmsis_os.h" |
chris | 0:22a5db2c7926 | 6 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 7 | #define tempo_period 50 //Periodo do PWM |
carlos_nascimento08 | 3:10da2a723362 | 8 | #define diametro_direita 40.50 //mm |
carlos_nascimento08 | 3:10da2a723362 | 9 | #define diametro_esquerda 40.50 //mm |
carlos_nascimento08 | 3:10da2a723362 | 10 | #define pi 3.1415926535897 |
carlos_nascimento08 | 2:f1f6b33cd7bd | 11 | #define velocidade_max 220.00 //cm/seg |
carlos_nascimento08 | 3:10da2a723362 | 12 | #define dist_minima 7.0 //cm |
carlos_nascimento08 | 2:f1f6b33cd7bd | 13 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 14 | TextLCD lcd(p5, p6, p7, p8, p29, p30, TextLCD::LCD20x4); // rs, e, d4-d7 |
carlos_nascimento08 | 3:10da2a723362 | 15 | RoboClaw Placa; |
carlos_nascimento08 | 3:10da2a723362 | 16 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 17 | MSCFileSystem sd("sd"); |
chris | 0:22a5db2c7926 | 18 | |
carlos_nascimento08 | 3:10da2a723362 | 19 | Timer tempo; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 20 | |
chris | 0:22a5db2c7926 | 21 | DigitalOut led1(LED1); |
chris | 0:22a5db2c7926 | 22 | DigitalOut led2(LED2); |
chris | 0:22a5db2c7926 | 23 | DigitalOut led3(LED3); |
chris | 0:22a5db2c7926 | 24 | DigitalOut led4(LED4); |
chris | 0:22a5db2c7926 | 25 | |
carlos_nascimento08 | 3:10da2a723362 | 26 | // Variavel de leitura de distancia do sensor Sharp |
carlos_nascimento08 | 3:10da2a723362 | 27 | float valor1 = 0, valor2 = 0, valor3 = 0, valor4 = 0; |
carlos_nascimento08 | 3:10da2a723362 | 28 | |
carlos_nascimento08 | 3:10da2a723362 | 29 | // Leitura de dados Pendrive |
carlos_nascimento08 | 3:10da2a723362 | 30 | float read_tempo = 0; |
chris | 0:22a5db2c7926 | 31 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 32 | //Declaracao de variaveis universal RoboClaw: |
carlos_nascimento08 | 2:f1f6b33cd7bd | 33 | char endereco = 128; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 34 | char valor = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 35 | char check8 = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 36 | char buffer = 0; |
carlos_nascimento08 | 3:10da2a723362 | 37 | char status_encoder_e = 0; |
carlos_nascimento08 | 3:10da2a723362 | 38 | char status_encoder_d = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 39 | unsigned int aceleracao = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 40 | int velocidade_e = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 41 | int velocidade_d = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 42 | unsigned int cmd_distancia_e = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 43 | unsigned int cmd_distancia_d = 0; |
carlos_nascimento08 | 3:10da2a723362 | 44 | int distancia_e = 0; |
carlos_nascimento08 | 3:10da2a723362 | 45 | int distancia_d = 0; |
carlos_nascimento08 | 3:10da2a723362 | 46 | int temp_distancia_e = 0; |
carlos_nascimento08 | 3:10da2a723362 | 47 | int temp_distancia_d = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 48 | unsigned int Kp = 65536; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 49 | unsigned int Ki = 32768; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 50 | unsigned int Kd = 16384; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 51 | unsigned int QPPS = 800; |
carlos_nascimento08 | 3:10da2a723362 | 52 | unsigned long long resposta_encoder = 0; |
carlos_nascimento08 | 3:10da2a723362 | 53 | unsigned short le_buffer = 0; |
carlos_nascimento08 | 3:10da2a723362 | 54 | char buffer_M1 = 0, buffer_M2 = 0; |
carlos_nascimento08 | 3:10da2a723362 | 55 | char usando_na_thread = 0; |
carlos_nascimento08 | 3:10da2a723362 | 56 | char usando_na_main = 1; |
chris | 0:22a5db2c7926 | 57 | |
chris | 0:22a5db2c7926 | 58 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 59 | /* |
carlos_nascimento08 | 2:f1f6b33cd7bd | 60 | //Rotina Interrupcao Thread -> Controle de Velocidade |
carlos_nascimento08 | 2:f1f6b33cd7bd | 61 | void thread_interrompe(void const *argument) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 62 | { |
carlos_nascimento08 | 3:10da2a723362 | 63 | char parar = 0; |
carlos_nascimento08 | 3:10da2a723362 | 64 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 65 | while (true) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 66 | { |
carlos_nascimento08 | 3:10da2a723362 | 67 | valor1 = Placa.sharp(1); |
carlos_nascimento08 | 3:10da2a723362 | 68 | valor2 = Placa.sharp(2); |
carlos_nascimento08 | 3:10da2a723362 | 69 | valor3 = Placa.sharp(3); |
carlos_nascimento08 | 3:10da2a723362 | 70 | valor4 = Placa.sharp(4); |
carlos_nascimento08 | 3:10da2a723362 | 71 | |
carlos_nascimento08 | 3:10da2a723362 | 72 | if(((valor1 <= dist_minima) && (valor1 >=0)) || ((valor2 <= dist_minima) && (valor2 >=0)) || ((valor3 <= dist_minima) && (valor3 >=0)) || ((valor4 <= dist_minima) && (valor4 >=0))) |
carlos_nascimento08 | 3:10da2a723362 | 73 | { |
carlos_nascimento08 | 3:10da2a723362 | 74 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 75 | parar = 0; |
carlos_nascimento08 | 3:10da2a723362 | 76 | Placa.comando8(endereco, parar); |
carlos_nascimento08 | 3:10da2a723362 | 77 | lcd.cls(); |
carlos_nascimento08 | 3:10da2a723362 | 78 | lcd.printf("1/%4.2f 2/%4.2f\n3/%4.2f 4/%4.2f" , valor1, valor2, valor3, valor4); |
carlos_nascimento08 | 3:10da2a723362 | 79 | wait(2); |
carlos_nascimento08 | 3:10da2a723362 | 80 | } |
carlos_nascimento08 | 3:10da2a723362 | 81 | osDelay(0.1); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 82 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 83 | } |
carlos_nascimento08 | 3:10da2a723362 | 84 | osThreadDef(thread_interrompe, osPriorityNormal, DEFAULT_STACK_SIZE); */ |
carlos_nascimento08 | 2:f1f6b33cd7bd | 85 | |
carlos_nascimento08 | 3:10da2a723362 | 86 | /* |
carlos_nascimento08 | 2:f1f6b33cd7bd | 87 | //Rotina Interrupcao Thread -> Atualiza vetor de tempo |
carlos_nascimento08 | 2:f1f6b33cd7bd | 88 | void vetor_atualiza(void const *argument) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 89 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 90 | while (true) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 91 | { |
carlos_nascimento08 | 3:10da2a723362 | 92 | if(usando_na_main != 1) |
carlos_nascimento08 | 3:10da2a723362 | 93 | { |
carlos_nascimento08 | 3:10da2a723362 | 94 | usando_na_thread = 1; |
carlos_nascimento08 | 3:10da2a723362 | 95 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 96 | le_buffer = Placa.comando47(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 97 | buffer_M1 = ((le_buffer>>8) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 98 | buffer_M2 = (le_buffer & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 99 | usando_na_thread = 0; |
carlos_nascimento08 | 3:10da2a723362 | 100 | } |
carlos_nascimento08 | 3:10da2a723362 | 101 | osDelay(0.1); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 102 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 103 | } |
carlos_nascimento08 | 3:10da2a723362 | 104 | osThreadDef(vetor_atualiza, osPriorityNormal, DEFAULT_STACK_SIZE);*/ |
carlos_nascimento08 | 3:10da2a723362 | 105 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 106 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 107 | ////////////////////////////////////////////////////////////////// |
carlos_nascimento08 | 2:f1f6b33cd7bd | 108 | ////////////////////////////////////////////////////////////////// |
carlos_nascimento08 | 2:f1f6b33cd7bd | 109 | int main() |
carlos_nascimento08 | 3:10da2a723362 | 110 | { |
carlos_nascimento08 | 3:10da2a723362 | 111 | |
carlos_nascimento08 | 3:10da2a723362 | 112 | FILE *fp = fopen("/sd/dad.txt","w"); |
carlos_nascimento08 | 3:10da2a723362 | 113 | if ( fp == NULL ) |
carlos_nascimento08 | 3:10da2a723362 | 114 | { |
carlos_nascimento08 | 3:10da2a723362 | 115 | error("Could not open file for write\n"); |
carlos_nascimento08 | 3:10da2a723362 | 116 | led2 = 1; |
carlos_nascimento08 | 3:10da2a723362 | 117 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 118 | |
carlos_nascimento08 | 3:10da2a723362 | 119 | usando_na_main = 1; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 120 | // Comeca Thread |
carlos_nascimento08 | 2:f1f6b33cd7bd | 121 | //osThreadCreate(osThread(thread_interrompe), NULL); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 122 | //osThreadCreate(osThread(vetor_atualiza), NULL); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 123 | |
carlos_nascimento08 | 3:10da2a723362 | 124 | wait(1); |
carlos_nascimento08 | 3:10da2a723362 | 125 | |
carlos_nascimento08 | 3:10da2a723362 | 126 | //Minimo volts main battery: 6V |
carlos_nascimento08 | 3:10da2a723362 | 127 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 128 | valor = 0; |
carlos_nascimento08 | 3:10da2a723362 | 129 | Placa.comando2(endereco, valor); |
carlos_nascimento08 | 3:10da2a723362 | 130 | |
carlos_nascimento08 | 3:10da2a723362 | 131 | //Máximo volts main battery: 9V |
carlos_nascimento08 | 3:10da2a723362 | 132 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 133 | valor = 47; |
carlos_nascimento08 | 3:10da2a723362 | 134 | Placa.comando3(endereco, valor); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 135 | |
carlos_nascimento08 | 3:10da2a723362 | 136 | // Zera o encoder |
carlos_nascimento08 | 3:10da2a723362 | 137 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 138 | Placa.comando20(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 139 | |
carlos_nascimento08 | 3:10da2a723362 | 140 | // Configura o motor M1 = Direita |
carlos_nascimento08 | 3:10da2a723362 | 141 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 142 | Kp = 1000000; |
carlos_nascimento08 | 3:10da2a723362 | 143 | Ki = 500000; |
carlos_nascimento08 | 3:10da2a723362 | 144 | Kd = 250000; |
carlos_nascimento08 | 3:10da2a723362 | 145 | QPPS = 880; // 840+ 10% |
carlos_nascimento08 | 3:10da2a723362 | 146 | Placa.comando28(endereco, Kd, Kp, Ki,QPPS); |
carlos_nascimento08 | 3:10da2a723362 | 147 | |
carlos_nascimento08 | 3:10da2a723362 | 148 | // Configura o motor M2 = Esquerda |
carlos_nascimento08 | 3:10da2a723362 | 149 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 150 | Kp = 1000000; |
carlos_nascimento08 | 3:10da2a723362 | 151 | Ki = 500000; |
carlos_nascimento08 | 3:10da2a723362 | 152 | Kd = 250000; |
carlos_nascimento08 | 3:10da2a723362 | 153 | QPPS = 880; // 840+ 10% |
carlos_nascimento08 | 3:10da2a723362 | 154 | Placa.comando29(endereco, Kd, Kp, Ki,QPPS); |
carlos_nascimento08 | 3:10da2a723362 | 155 | |
chris | 0:22a5db2c7926 | 156 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 157 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 158 | aceleracao = 50; |
carlos_nascimento08 | 3:10da2a723362 | 159 | velocidade_e = 50; |
carlos_nascimento08 | 3:10da2a723362 | 160 | velocidade_d = 50; |
carlos_nascimento08 | 3:10da2a723362 | 161 | cmd_distancia_e = 179; |
carlos_nascimento08 | 3:10da2a723362 | 162 | cmd_distancia_d = 179; |
carlos_nascimento08 | 3:10da2a723362 | 163 | buffer = 1; |
carlos_nascimento08 | 3:10da2a723362 | 164 | //Placa.comando37(endereco, velocidade_d, velocidade_e); |
carlos_nascimento08 | 3:10da2a723362 | 165 | Placa.comando46(endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_e, cmd_distancia_e, buffer); |
carlos_nascimento08 | 3:10da2a723362 | 166 | |
carlos_nascimento08 | 3:10da2a723362 | 167 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 168 | aceleracao = 125; |
carlos_nascimento08 | 3:10da2a723362 | 169 | velocidade_e = 0; |
carlos_nascimento08 | 3:10da2a723362 | 170 | velocidade_d = 0; |
carlos_nascimento08 | 3:10da2a723362 | 171 | cmd_distancia_e = 10; |
carlos_nascimento08 | 3:10da2a723362 | 172 | cmd_distancia_d = 10; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 173 | buffer = 0; |
carlos_nascimento08 | 3:10da2a723362 | 174 | Placa.comando46(endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_e, cmd_distancia_e, buffer); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 175 | |
carlos_nascimento08 | 3:10da2a723362 | 176 | // Monitoramento no Pendrive |
carlos_nascimento08 | 3:10da2a723362 | 177 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 178 | resposta_encoder = Placa.comando18(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 179 | velocidade_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 180 | resposta_encoder = Placa.comando19(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 181 | velocidade_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 182 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 183 | |
carlos_nascimento08 | 3:10da2a723362 | 184 | while((velocidade_d==0) || (velocidade_e==0)) |
carlos_nascimento08 | 3:10da2a723362 | 185 | { |
carlos_nascimento08 | 3:10da2a723362 | 186 | resposta_encoder = Placa.comando18(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 187 | velocidade_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 188 | status_encoder_d = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 189 | |
carlos_nascimento08 | 3:10da2a723362 | 190 | resposta_encoder = Placa.comando19(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 191 | velocidade_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 192 | status_encoder_e = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 193 | } |
carlos_nascimento08 | 3:10da2a723362 | 194 | |
carlos_nascimento08 | 3:10da2a723362 | 195 | tempo.start(); |
carlos_nascimento08 | 3:10da2a723362 | 196 | |
carlos_nascimento08 | 3:10da2a723362 | 197 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 198 | resposta_encoder = Placa.comando18(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 199 | velocidade_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 200 | resposta_encoder = Placa.comando19(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 201 | velocidade_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 202 | |
carlos_nascimento08 | 3:10da2a723362 | 203 | while((velocidade_d!=0) || (velocidade_e!=0)) |
carlos_nascimento08 | 3:10da2a723362 | 204 | { |
carlos_nascimento08 | 3:10da2a723362 | 205 | led4 != led4; |
carlos_nascimento08 | 3:10da2a723362 | 206 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 207 | resposta_encoder = Placa.comando16(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 208 | distancia_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 209 | status_encoder_d = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 210 | |
carlos_nascimento08 | 3:10da2a723362 | 211 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 212 | resposta_encoder = Placa.comando17(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 213 | distancia_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 214 | status_encoder_e = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 215 | |
carlos_nascimento08 | 3:10da2a723362 | 216 | resposta_encoder = Placa.comando18(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 217 | velocidade_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 218 | status_encoder_d = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 219 | |
carlos_nascimento08 | 3:10da2a723362 | 220 | resposta_encoder = Placa.comando19(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 221 | velocidade_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 222 | status_encoder_e = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 223 | |
carlos_nascimento08 | 3:10da2a723362 | 224 | read_tempo = tempo.read_ms(); |
carlos_nascimento08 | 3:10da2a723362 | 225 | |
carlos_nascimento08 | 3:10da2a723362 | 226 | fprintf(fp, "%f\t" , read_tempo); |
carlos_nascimento08 | 3:10da2a723362 | 227 | fprintf(fp, "%d\t" , velocidade_e); |
carlos_nascimento08 | 3:10da2a723362 | 228 | fprintf(fp, "%d\t\t" , distancia_e); |
carlos_nascimento08 | 3:10da2a723362 | 229 | |
carlos_nascimento08 | 3:10da2a723362 | 230 | fprintf(fp, "%f\t" , read_tempo); |
carlos_nascimento08 | 3:10da2a723362 | 231 | fprintf(fp, "%d\t" , velocidade_d); |
carlos_nascimento08 | 3:10da2a723362 | 232 | fprintf(fp, "%d-" , distancia_d); |
carlos_nascimento08 | 3:10da2a723362 | 233 | |
carlos_nascimento08 | 3:10da2a723362 | 234 | wait_us(800); |
carlos_nascimento08 | 3:10da2a723362 | 235 | } |
carlos_nascimento08 | 3:10da2a723362 | 236 | |
carlos_nascimento08 | 3:10da2a723362 | 237 | fclose(fp); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 238 | |
carlos_nascimento08 | 3:10da2a723362 | 239 | led4 = 1; |
carlos_nascimento08 | 3:10da2a723362 | 240 | |
carlos_nascimento08 | 3:10da2a723362 | 241 | |
carlos_nascimento08 | 3:10da2a723362 | 242 | /* |
carlos_nascimento08 | 3:10da2a723362 | 243 | while(1) |
carlos_nascimento08 | 3:10da2a723362 | 244 | { |
carlos_nascimento08 | 3:10da2a723362 | 245 | valor1 = Placa.sharp(1); |
carlos_nascimento08 | 3:10da2a723362 | 246 | valor2 = Placa.sharp(2); |
carlos_nascimento08 | 3:10da2a723362 | 247 | valor3 = Placa.sharp(3); |
carlos_nascimento08 | 3:10da2a723362 | 248 | valor4 = Placa.sharp(4); |
carlos_nascimento08 | 3:10da2a723362 | 249 | |
carlos_nascimento08 | 3:10da2a723362 | 250 | led3 = !led3; |
carlos_nascimento08 | 3:10da2a723362 | 251 | |
carlos_nascimento08 | 3:10da2a723362 | 252 | lcd.cls(); |
carlos_nascimento08 | 3:10da2a723362 | 253 | lcd.printf("1/%4.2f 2/%4.2f\n3/%4.2f 4/%4.2f" , valor1, valor2, valor3, valor4); |
carlos_nascimento08 | 3:10da2a723362 | 254 | wait(0.1); |
carlos_nascimento08 | 3:10da2a723362 | 255 | |
carlos_nascimento08 | 3:10da2a723362 | 256 | |
carlos_nascimento08 | 3:10da2a723362 | 257 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 258 | resposta_encoder = Placa.comando16(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 259 | distancia_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 260 | status_encoder_d = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 261 | |
carlos_nascimento08 | 3:10da2a723362 | 262 | endereco = 128; |
carlos_nascimento08 | 3:10da2a723362 | 263 | resposta_encoder = Placa.comando17(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 264 | distancia_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 265 | status_encoder_e = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 266 | |
carlos_nascimento08 | 3:10da2a723362 | 267 | resposta_encoder = Placa.comando18(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 268 | velocidade_d = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 269 | status_encoder_d = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 3:10da2a723362 | 270 | |
carlos_nascimento08 | 3:10da2a723362 | 271 | resposta_encoder = Placa.comando19(endereco); |
carlos_nascimento08 | 3:10da2a723362 | 272 | velocidade_e = int (resposta_encoder & 0xFFFFFFFF); |
carlos_nascimento08 | 3:10da2a723362 | 273 | status_encoder_e = char ((resposta_encoder>>32) & 0xFF); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 274 | |
carlos_nascimento08 | 3:10da2a723362 | 275 | lcd.cls(); |
carlos_nascimento08 | 3:10da2a723362 | 276 | lcd.printf("DD:%i*DE:%i" , distancia_d, distancia_e); |
carlos_nascimento08 | 3:10da2a723362 | 277 | lcd.printf("\nVD:%i*VE:%i" , velocidade_d, velocidade_e); |
carlos_nascimento08 | 3:10da2a723362 | 278 | wait(0.2); |
carlos_nascimento08 | 3:10da2a723362 | 279 | } |
carlos_nascimento08 | 3:10da2a723362 | 280 | */ |
carlos_nascimento08 | 2:f1f6b33cd7bd | 281 | } |