Carlos Alberto Nascimento Filho / Mbed 2 deprecated Andar_Linha_Reta_Robo_Claw_v1

Program version 1

Dependencies:   FatFileSystemSD MSCFileSystem TextLCD mbed-rtos mbed

Fork of USBFlashDiskTest by Chris Styles

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?

UserRevisionLine numberNew 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 }