main.cpp

00001 #include "mbed.h"
00002 #include "RoboClaw.h"
00003 #include "TextLCD.h"
00004 #include "MSCFileSystem.h"
00005 #include "cmsis_os.h"
00006 
00007 #define tempo_period 50 //Periodo do PWM
00008 #define diametro_direita 40.50 //mm
00009 #define diametro_esquerda 40.50 //mm
00010 #define pi 3.1415926535897 
00011 #define velocidade_max 220.00 //cm/seg
00012 #define dist_minima 7.0 //cm
00013  
00014 TextLCD lcd(p5, p6, p7, p8, p29, p30, TextLCD::LCD20x4); // rs, e, d4-d7
00015 RoboClaw Placa;
00016 
00017 MSCFileSystem  sd("sd");
00018 
00019 Timer tempo;
00020 
00021 DigitalOut led1(LED1);
00022 DigitalOut led2(LED2);
00023 DigitalOut led3(LED3);
00024 DigitalOut led4(LED4);
00025 
00026 // Variavel de leitura de distancia do sensor Sharp
00027 float valor1 = 0, valor2 = 0, valor3 = 0, valor4 = 0;
00028 
00029 // Leitura de dados Pendrive
00030 float read_tempo = 0;
00031 
00032 //Declaracao de variaveis universal RoboClaw:
00033 char endereco = 128;
00034 char valor = 0;
00035 char check8 = 0;    
00036 char buffer = 0;  
00037 char status_encoder_e = 0; 
00038 char status_encoder_d = 0;  
00039 unsigned int aceleracao = 0;
00040 int velocidade_e = 0;
00041 int velocidade_d = 0;
00042 unsigned int cmd_distancia_e = 0;
00043 unsigned int cmd_distancia_d = 0;
00044 int distancia_e = 0;
00045 int distancia_d = 0;
00046 int temp_distancia_e = 0;
00047 int temp_distancia_d = 0;
00048 unsigned int Kp = 65536;
00049 unsigned int Ki = 32768;
00050 unsigned int Kd = 16384;
00051 unsigned int QPPS = 800;
00052 unsigned long long resposta_encoder = 0;
00053 unsigned short le_buffer = 0;
00054 char buffer_M1 = 0, buffer_M2 = 0;
00055 char usando_na_thread = 0;
00056 char usando_na_main = 1;
00057 
00058 
00059 /*
00060 //Rotina Interrupcao Thread -> Controle de Velocidade
00061 void thread_interrompe(void const *argument) 
00062 {
00063     char parar = 0;
00064     
00065     while (true) 
00066     {
00067         valor1 = Placa.sharp(1);
00068         valor2 = Placa.sharp(2);
00069         valor3 = Placa.sharp(3);
00070         valor4 = Placa.sharp(4);
00071 
00072         if(((valor1 <= dist_minima) && (valor1 >=0)) || ((valor2 <= dist_minima) && (valor2 >=0)) || ((valor3 <= dist_minima) && (valor3 >=0)) || ((valor4 <= dist_minima) && (valor4 >=0)))
00073         {
00074             endereco = 128;
00075             parar = 0;
00076             Placa.comando8(endereco, parar);
00077             lcd.cls();
00078             lcd.printf("1/%4.2f 2/%4.2f\n3/%4.2f 4/%4.2f" , valor1, valor2, valor3, valor4);
00079             wait(2);
00080         }      
00081         osDelay(0.1);
00082     }
00083 }
00084 osThreadDef(thread_interrompe, osPriorityNormal, DEFAULT_STACK_SIZE);   */ 
00085 
00086 /*
00087 //Rotina Interrupcao Thread -> Atualiza vetor de tempo
00088 void vetor_atualiza(void const *argument) 
00089 {
00090     while (true) 
00091     {
00092         if(usando_na_main != 1)
00093         {
00094             usando_na_thread = 1;
00095             endereco = 128;
00096             le_buffer = Placa.comando47(endereco);
00097             buffer_M1 = ((le_buffer>>8) & 0xFF);
00098             buffer_M2 = (le_buffer & 0xFF);
00099             usando_na_thread = 0;
00100         }                
00101         osDelay(0.1);
00102     }
00103 }
00104 osThreadDef(vetor_atualiza, osPriorityNormal, DEFAULT_STACK_SIZE);*/
00105 
00106 
00107 //////////////////////////////////////////////////////////////////
00108 //////////////////////////////////////////////////////////////////
00109 int main() 
00110 { 
00111 
00112     FILE *fp = fopen("/sd/dad.txt","w");
00113     if ( fp == NULL )
00114     {
00115         error("Could not open file for write\n");
00116         led2 = 1;
00117     }
00118    
00119     usando_na_main = 1;
00120     // Comeca Thread
00121     //osThreadCreate(osThread(thread_interrompe), NULL);
00122     //osThreadCreate(osThread(vetor_atualiza), NULL);
00123     
00124     wait(1);
00125                                 
00126     //Minimo volts main battery: 6V
00127     endereco = 128;
00128     valor = 0;
00129     Placa.comando2(endereco, valor);
00130 
00131     //Máximo volts main battery: 9V
00132     endereco = 128;
00133     valor = 47;
00134     Placa.comando3(endereco, valor); 
00135     
00136     // Zera o encoder
00137     endereco = 128;
00138     Placa.comando20(endereco);
00139     
00140     // Configura o motor M1 = Direita
00141     endereco = 128;
00142     Kp = 1000000;       
00143     Ki = 500000;
00144     Kd = 250000;
00145     QPPS = 880;  // 840+ 10%
00146     Placa.comando28(endereco, Kd, Kp, Ki,QPPS);
00147 
00148     // Configura o motor M2 = Esquerda
00149     endereco = 128;
00150     Kp = 1000000;       
00151     Ki = 500000;
00152     Kd = 250000;
00153     QPPS = 880;  // 840+ 10%
00154     Placa.comando29(endereco, Kd, Kp, Ki,QPPS); 
00155 
00156 
00157     endereco = 128;  
00158     aceleracao = 50;
00159     velocidade_e = 50;
00160     velocidade_d = 50;
00161     cmd_distancia_e = 179;
00162     cmd_distancia_d = 179;
00163     buffer = 1;
00164     //Placa.comando37(endereco, velocidade_d, velocidade_e);
00165     Placa.comando46(endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_e, cmd_distancia_e, buffer);
00166 
00167     endereco = 128;  
00168     aceleracao = 125;
00169     velocidade_e = 0;
00170     velocidade_d = 0;
00171     cmd_distancia_e = 10;
00172     cmd_distancia_d = 10;
00173     buffer = 0;
00174     Placa.comando46(endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_e, cmd_distancia_e, buffer);
00175 
00176     // Monitoramento no Pendrive
00177     endereco = 128;
00178     resposta_encoder = Placa.comando18(endereco);
00179     velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
00180     resposta_encoder = Placa.comando19(endereco);
00181     velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
00182 
00183 
00184     while((velocidade_d==0) || (velocidade_e==0))
00185     {
00186         resposta_encoder = Placa.comando18(endereco);
00187         velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
00188         status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
00189 
00190         resposta_encoder = Placa.comando19(endereco);
00191         velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
00192         status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
00193     }
00194     
00195     tempo.start();
00196     
00197     endereco = 128;
00198     resposta_encoder = Placa.comando18(endereco);
00199     velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
00200     resposta_encoder = Placa.comando19(endereco);
00201     velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
00202     
00203     while((velocidade_d!=0) || (velocidade_e!=0))
00204     {
00205         led4 != led4;
00206         endereco = 128;
00207         resposta_encoder = Placa.comando16(endereco);
00208         distancia_d = int (resposta_encoder & 0xFFFFFFFF);
00209         status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
00210     
00211         endereco = 128;
00212         resposta_encoder = Placa.comando17(endereco);
00213         distancia_e = int (resposta_encoder & 0xFFFFFFFF);
00214         status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
00215             
00216         resposta_encoder = Placa.comando18(endereco);
00217         velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
00218         status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
00219 
00220         resposta_encoder = Placa.comando19(endereco);
00221         velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
00222         status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
00223         
00224         read_tempo = tempo.read_ms();
00225         
00226         fprintf(fp, "%f\t" , read_tempo);
00227         fprintf(fp, "%d\t" , velocidade_e);
00228         fprintf(fp, "%d\t\t" , distancia_e);
00229         
00230         fprintf(fp, "%f\t" , read_tempo);
00231         fprintf(fp, "%d\t" , velocidade_d);
00232         fprintf(fp, "%d-" , distancia_d);
00233         
00234         wait_us(800);   
00235     }
00236         
00237     fclose(fp);
00238     
00239     led4 = 1;
00240 
00241 
00242     /*    
00243     while(1)
00244     {
00245         valor1 = Placa.sharp(1);
00246         valor2 = Placa.sharp(2);
00247         valor3 = Placa.sharp(3);
00248         valor4 = Placa.sharp(4);
00249 
00250         led3 = !led3;
00251                 
00252         lcd.cls();
00253         lcd.printf("1/%4.2f 2/%4.2f\n3/%4.2f 4/%4.2f" , valor1, valor2, valor3, valor4);
00254         wait(0.1);
00255 
00256         
00257         endereco = 128;
00258         resposta_encoder = Placa.comando16(endereco);
00259         distancia_d = int (resposta_encoder & 0xFFFFFFFF);
00260         status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
00261     
00262         endereco = 128;
00263         resposta_encoder = Placa.comando17(endereco);
00264         distancia_e = int (resposta_encoder & 0xFFFFFFFF);
00265         status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
00266             
00267         resposta_encoder = Placa.comando18(endereco);
00268         velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
00269         status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
00270 
00271         resposta_encoder = Placa.comando19(endereco);
00272         velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
00273         status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
00274     
00275         lcd.cls(); 
00276         lcd.printf("DD:%i*DE:%i" , distancia_d, distancia_e);
00277         lcd.printf("\nVD:%i*VE:%i" , velocidade_d, velocidade_e);    
00278         wait(0.2); 
00279     } 
00280     */          
00281 }