Revision 3:2544cb29d82e, committed 2012-11-21

Comitter:

carlos_nascimento08

Date:

Wed Nov 21 17:18:43 2012 +0000

Parent:

2:15a805c1202a

Commit message:

Library personal

Changed in this revision

--- a/RoboClaw.cpp	Sun Sep 16 15:07:33 2012 +0000
+++ b/RoboClaw.cpp	Wed Nov 21 17:18:43 2012 +0000
@@ -1,87 +1,1104 @@
-/* mbed TextLCD Library, for RoboClaw 15A
- */
-
-#include "RoboClaw.h"
-#include "mbed.h"
-
-Serial Robo(p28, p27); // tx, rx
-char envia_32 (unsigned int valor);
-char envia_8 (unsigned int valor);
-
-
-char envia_8 (unsigned int valor)
-{
-    char soma = 0 , manda = 0;
-        
-    manda = (valor)&0xFF;
-    Robo.putc(manda);
-    soma = soma + manda;  
-
-    return soma;
-}
-
-char envia_32 (unsigned int valor)
-{
-    char soma = 0 , manda = 0;
-        
-    
-    manda = (valor>>24)&0xFF;
-    Robo.putc(manda);
-    soma = soma + manda;     
-
-    manda = (valor>>16)&0xFF;
-    Robo.putc(manda);
-    soma = soma + manda;  
-
-    manda = (valor>>8)&0xFF;
-    Robo.putc(manda); 
-    soma = soma + manda;
-
-    manda = (valor)&0xFF;
-    Robo.putc(manda);
-    soma = soma + manda;  
-
-    return soma;
-}
-
-
-RoboClaw::RoboClaw(void)
-{
-    Robo.baud(38400);
-}
-
-
-// Comandos:
-
-void RoboClaw::comando46 (char address, unsigned int accel, int qspeedM1, unsigned int distanceM1, int    qspeedM2, unsigned int distanceM2, char cbuffer)
-{
-    char resposta = 0, ccheck8 = 0;
-        
-    resposta = envia_8 (address);
-    ccheck8 = ccheck8 + resposta;
-
-    resposta = envia_8 (46);
-    ccheck8 = ccheck8 + resposta;
-    
-    resposta = envia_32 (accel);
-    ccheck8 = ccheck8 + resposta;
-    
-    resposta = envia_32 (qspeedM1);
-    ccheck8 = ccheck8 + resposta;
-    
-    resposta = envia_32 (distanceM1);
-    ccheck8 = ccheck8 + resposta;    
-    
-    resposta = envia_32 (qspeedM2);
-    ccheck8 = ccheck8 + resposta;
-    
-    resposta = envia_32 (distanceM2);
-    ccheck8 = ccheck8 + resposta;     
-
-    resposta = envia_8 (cbuffer);
-    ccheck8 = ccheck8 + resposta;
-
-    ccheck8 = ccheck8&0x7F;
-    
-    resposta = envia_8(ccheck8);
-}
+/* mbed TextLCD Library, for RoboClaw 15A
+ */
+
+#include "RoboClaw.h"
+#include "mbed.h"
+
+Serial Robo(p28, p27); // tx, rx
+
+AnalogIn sensor1(p15); 
+AnalogIn sensor2(p16); 
+AnalogIn sensor3(p17); 
+AnalogIn sensor4(p19); 
+
+char envia_8(unsigned int valor);
+char signed_envia_32(int valor);
+char unsigned_envia_32(unsigned int valor);
+double le_32_vezes(char valor);
+
+double le_32_vezes(char valor)
+{
+    double x = 0;
+    int i = 0;
+    
+    if(valor == 1)
+    {
+        for(i = 1; i<=32; i++)
+            x = x + sensor1;
+        x = 3.3*x;
+        x = x/32.0;
+        return (x);
+    }
+
+    if(valor == 2)
+    {
+        for(i = 1; i<=32; i++)
+            x = x + sensor2;
+        x = 3.3*x;
+        x = x/32.0;
+        return (x);
+    }
+
+    if(valor == 3)
+    {
+        for(i = 1; i<=32; i++)
+            x = x + sensor3;
+        x = 3.3*x;
+        x = x/32.0;
+        return (x);
+    }
+
+    if(valor == 4)
+    {
+        for(i = 1; i<=32; i++)
+            x = x + sensor4;
+        x = 3.3*x;
+        x = x/32.0;
+        return (x);
+    }
+    else
+        return (-1);
+}
+
+char envia_8(unsigned int valor)
+{
+    char soma = 0 , manda = 0;
+        
+    manda = (valor)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;  
+
+    return soma;
+}
+
+char signed_envia_32(int valor)
+{
+    char soma = 0 , manda = 0;
+    
+    manda = (valor>>24)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;     
+
+    manda = (valor>>16)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;  
+
+    manda = (valor>>8)&0xFF;
+    Robo.putc(manda); 
+    soma = soma + manda;
+
+    manda = (valor)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;  
+
+    return soma;
+}
+
+char unsigned_envia_32(unsigned int valor)
+{
+    char soma = 0 , manda = 0;
+    
+    manda = (valor>>24)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;     
+
+    manda = (valor>>16)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;  
+
+    manda = (valor>>8)&0xFF;
+    Robo.putc(manda); 
+    soma = soma + manda;
+
+    manda = (valor)&0xFF;
+    Robo.putc(manda);
+    soma = soma + manda;  
+
+    return soma;
+}
+
+
+RoboClaw::RoboClaw(void)
+{
+    wait(0.5);
+}
+
+
+// Comandos:
+void RoboClaw::comando0(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 0;
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+void RoboClaw::comando1(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 1;
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando2(char address, char value)
+{
+    // Minimo 7V -> (7-6)*5=5
+    char resposta = 0, ccheck8 = 0, cmd = 2;
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando3(char address, char value)
+{
+    // Maximo 8V -> 42
+    char resposta = 0, ccheck8 = 0, cmd = 3;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+void RoboClaw::comando4(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 4;
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+void RoboClaw::comando5(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 5;
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+void RoboClaw::comando8(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 8;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando9(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 9;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}    
+
+
+void RoboClaw::comando10(char address, char value) //virar para esquerda
+{
+    char resposta = 0, ccheck8 = 0, cmd = 10;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}    
+
+
+void RoboClaw::comando11(char address, char value) //virar para direita
+{
+    char resposta = 0, ccheck8 = 0, cmd = 11;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+void RoboClaw::comando13(char address, char value)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 13;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(value);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+/*
+        endereco = 128;
+        resposta_encoder = Placa.comando16(endereco);
+        distancia_d = int (resposta_encoder & 0xFFFFFFFF);
+        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
+    
+        endereco = 128;
+        resposta_encoder = Placa.comando17(endereco);
+        distancia_e = int (resposta_encoder & 0xFFFFFFFF);
+        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
+
+        resposta_encoder = Placa.comando18(endereco);
+        velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
+        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
+
+        resposta_encoder = Placa.comando19(endereco);
+        velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
+        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
+    
+        lcd.cls(); 
+        lcd.printf("DD:%i*DE:%i" , distancia_d, distancia_e);
+        lcd.printf("\nVD:%i*VE:%i" , velocidade_d, velocidade_e);    
+        wait(0.1);
+*/
+
+/*
+    endereco = 128;
+    resposta_encoder = Placa.comando16(endereco);
+    distancia_d = int (resposta_encoder & 0xFFFFFFFF);
+    status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
+
+    endereco = 128;
+    resposta_encoder = Placa.comando17(endereco);
+    distancia_e = int (resposta_encoder & 0xFFFFFFFF);
+    status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
+    
+    lcd.cls(); 
+    lcd.printf("D: %i\nE: %i" , distancia_d, distancia_e);
+    wait(0.5);  
+    // de acordo com o valor de "status_encoder_d" é possível verificar se le foi para frente ou para tras,......
+*/
+
+unsigned long long RoboClaw::comando16(char address)
+{
+    char cmd = 16, recebe[7], chegar = 0, checksum_recebido = 0, checksum = 0;
+    unsigned int i = 0;
+    unsigned long long dist = 0, aux = 0;  
+
+    Robo.baud(38400);
+
+
+    envia_8(address);
+    envia_8(cmd);
+    
+    for(i=1 ; i<=6 ; i++)
+    {
+        chegar = Robo.getc();
+        recebe[i] = chegar; 
+    }
+    
+    aux = recebe[1];
+    aux = (aux<<24);
+    dist = dist + aux;
+    aux = recebe[2];
+    aux = (aux<<16);     
+    dist = dist + aux;
+    aux = recebe[3];
+    aux = (aux<<8);     
+    dist = dist + aux;
+    aux = recebe[4]; 
+    dist = dist + aux;
+
+    aux = recebe[5]; //É o byte 5, status do encoder
+    aux = (aux<<32);
+    dist = dist + aux;
+
+    checksum_recebido = (recebe[6] - 0x10)&0x7F; // Tem que subtrair erro da placa de controle
+
+    checksum = 0;
+    for(i=1 ; i<=5 ; i++)
+    {
+        chegar = recebe[i]; 
+        checksum = checksum + chegar;
+    }
+    checksum = checksum&0x7F;
+
+    if(checksum == checksum_recebido)
+        return (dist);
+    else
+        return (0);
+}
+
+
+unsigned long long RoboClaw::comando17(char address)
+{
+    char cmd = 17, recebe[7], chegar = 0, checksum_recebido = 0, checksum = 0;
+    unsigned int i = 0;
+    unsigned long long dist = 0, aux = 0;  
+
+    Robo.baud(38400);
+
+    envia_8(address);
+    envia_8(cmd);
+    
+    for(i=1 ; i<=6 ; i++)
+    {
+        chegar = Robo.getc();
+        recebe[i] = chegar; 
+    }
+    
+    aux = recebe[1];
+    aux = (aux<<24);
+    dist = dist + aux;
+    aux = recebe[2];
+    aux = (aux<<16);     
+    dist = dist + aux;
+    aux = recebe[3];
+    aux = (aux<<8);     
+    dist = dist + aux;
+    aux = recebe[4]; 
+    dist = dist + aux;
+
+    aux = recebe[5]; //É o byte 5, status do encoder
+    aux = (aux<<32);
+    dist = dist + aux;
+
+    checksum_recebido = (recebe[6] - 0x11)&0x7F; // Tem que subtrair erro da placa de controle 
+
+    checksum = 0;
+    for(i=1 ; i<=5 ; i++)
+    {
+        chegar = recebe[i]; 
+        checksum = checksum + chegar;
+    }
+    checksum = checksum&0x7F;
+
+    if(checksum == checksum_recebido)
+        return (dist);
+    else
+        return (0);
+}
+
+/*
+    Modelo de leitura de velocidade de ambos motores:
+
+    endereco = 128;
+    resposta_encoder = Placa.comando18(endereco);
+    velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
+    status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
+    if(status_encoder_d)
+    {
+        velocidade_d = -velocidade_d;       
+    }
+
+    lcd.cls(); 
+    lcd.printf("D: %i %x\nE: %i %x" , velocidade_d, status_encoder_d , velocidade_e, status_encoder_e); 
+*/
+
+unsigned long long RoboClaw::comando18(char address)
+{
+    char cmd = 18, recebe[7], chegar = 0, checksum_recebido = 0, checksum = 0;
+    unsigned int i = 0;
+    unsigned long long dist = 0, aux = 0;  
+
+    Robo.baud(38400);
+
+
+    envia_8(address);
+    envia_8(cmd);
+    
+    for(i=1 ; i<=6 ; i++)
+    {
+        chegar = Robo.getc();
+        recebe[i] = chegar; 
+    }
+    
+    aux = recebe[1];
+    aux = (aux<<24);
+    dist = dist + aux;
+    aux = recebe[2];
+    aux = (aux<<16);     
+    dist = dist + aux;
+    aux = recebe[3];
+    aux = (aux<<8);     
+    dist = dist + aux;
+    aux = recebe[4]; 
+    dist = dist + aux;
+
+    aux = recebe[5]; //É o byte 5, status da Velocidade
+    aux = (aux<<32);
+    dist = dist + aux;
+
+    checksum_recebido = (recebe[6] + 0x6E)&0x7F; // Tem que subtrair erro da placa de controle 
+
+    checksum = 0;
+    for(i=1 ; i<=5 ; i++)
+    {
+        chegar = recebe[i]; 
+        checksum = checksum + chegar;
+    }
+    checksum = checksum&0x7F;
+
+    if(checksum == checksum_recebido)
+        return (dist);
+    else
+        return (0);
+}
+
+
+
+unsigned long long RoboClaw::comando19(char address)
+{
+    char cmd = 19, recebe[7], chegar = 0, checksum_recebido = 0, checksum = 0;
+    unsigned int i = 0;
+    unsigned long long dist = 0, aux = 0;  
+
+    Robo.baud(38400);
+
+
+    envia_8(address);
+    envia_8(cmd);
+    
+    for(i=1 ; i<=6 ; i++)
+    {
+        chegar = Robo.getc();
+        recebe[i] = chegar; 
+    }
+    
+    aux = recebe[1];
+    aux = (aux<<24);
+    dist = dist + aux;
+    aux = recebe[2];
+    aux = (aux<<16);     
+    dist = dist + aux;
+    aux = recebe[3];
+    aux = (aux<<8);     
+    dist = dist + aux;
+    aux = recebe[4]; 
+    dist = dist + aux;
+
+    aux = recebe[5]; //É o byte 5, status da Velocidade
+    aux = (aux<<32);
+    dist = dist + aux;
+
+    checksum_recebido = (recebe[6] + 0x6D)&0x7F; // Tem que subtrair erro da placa de controle 
+
+    checksum = 0;
+    for(i=1 ; i<=5 ; i++)
+    {
+        chegar = recebe[i]; 
+        checksum = checksum + chegar;
+    }
+    checksum = checksum&0x7F;
+
+    if(checksum == checksum_recebido)
+        return (dist);
+    else
+        return (0);
+}
+
+
+void RoboClaw::comando20(char address)
+{
+    // Maximo 8V -> 42
+    char resposta = 0, ccheck8 = 0, cmd = 20;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando28(char address, unsigned int D, unsigned int P, unsigned int I, unsigned int QPPS)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 28;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(D);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(P);
+    ccheck8 = ccheck8 + resposta;    
+    
+    resposta = unsigned_envia_32(I);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(QPPS);
+    ccheck8 = ccheck8 + resposta;     
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando29(char address, unsigned int D, unsigned int P, unsigned int I, unsigned int QPPS)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 29;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(D);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(P);
+    ccheck8 = ccheck8 + resposta;    
+    
+    resposta = unsigned_envia_32(I);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(QPPS);
+    ccheck8 = ccheck8 + resposta;     
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}            
+
+void RoboClaw::comando35(char address, int qspeedM1)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 35;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+                            
+    resposta = envia_8(ccheck8);
+}
+             
+
+void RoboClaw::comando36(char address, int qspeedM2)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 36;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+                            
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando37(char address, int qspeedM1, int qspeedM2)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 37;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+                            
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando38(char address, unsigned int accel, int qspeedM1)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 38;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = unsigned_envia_32(accel);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}  
+
+
+
+void RoboClaw::comando39(char address, unsigned int accel, int qspeedM2)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 39;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = unsigned_envia_32(accel);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}  
+
+
+void RoboClaw::comando40(char address, unsigned int accel, int qspeedM1, int qspeedM2)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 40;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = unsigned_envia_32(accel);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;    
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando41(char address, int qspeedM1, unsigned int distanceM1, char cbuffer)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 41;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM1);
+    ccheck8 = ccheck8 + resposta;    
+
+    resposta = envia_8(cbuffer);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando42(char address, int qspeedM2, unsigned int distanceM2, char cbuffer)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 42;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM2);
+    ccheck8 = ccheck8 + resposta;     
+
+    resposta = envia_8(cbuffer);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando43(char address, int qspeedM1, unsigned int distanceM1, int qspeedM2, unsigned int distanceM2, char cbuffer)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 43;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM1);
+    ccheck8 = ccheck8 + resposta;    
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM2);
+    ccheck8 = ccheck8 + resposta;     
+
+    resposta = envia_8(cbuffer);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}
+
+
+void RoboClaw::comando44(char address, unsigned int accel, int qspeedM1, unsigned int distanceM1, char cbuffer)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 44;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = unsigned_envia_32(accel);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM1);
+    ccheck8 = ccheck8 + resposta;    
+
+    resposta = envia_8(cbuffer);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}  
+
+
+
+void RoboClaw::comando45(char address, unsigned int accel, int qspeedM2, unsigned int distanceM2, char cbuffer)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 45;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = unsigned_envia_32(accel);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM2);
+    ccheck8 = ccheck8 + resposta;     
+
+    resposta = envia_8(cbuffer);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}  
+
+
+void RoboClaw::comando46(char address, unsigned int accel, int qspeedM1, unsigned int distanceM1, int qspeedM2, unsigned int distanceM2, char cbuffer)
+{
+    char resposta = 0, ccheck8 = 0, cmd = 46;  
+
+    Robo.baud(38400);
+      
+    resposta = envia_8(address);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = envia_8(cmd);
+    ccheck8 = ccheck8 + resposta;
+
+    resposta = unsigned_envia_32(accel);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = signed_envia_32(qspeedM1);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM1);
+    ccheck8 = ccheck8 + resposta;    
+    
+    resposta = signed_envia_32(qspeedM2);
+    ccheck8 = ccheck8 + resposta;
+    
+    resposta = unsigned_envia_32(distanceM2);
+    ccheck8 = ccheck8 + resposta;     
+
+    resposta = envia_8(cbuffer);
+    ccheck8 = ccheck8 + resposta;
+
+    ccheck8 = ccheck8&0x7F;
+    
+    resposta = envia_8(ccheck8);
+}  
+
+/*
+Padrao de leitura:
+- Quando fica 0x80=128 ou 0x00 o buffer está vazio
+
+endereco = 128;
+le_buffer = Placa.comando47(endereco);
+buffer_M1 = ((le_buffer>>8) & 0xFF);
+buffer_M2 = (le_buffer & 0xFF);
+
+
+lcd.cls(); 
+lcd.printf("M1: %i\nM2: %i" , buffer_M1, buffer_M2);
+wait(0.5);
+*/
+unsigned short RoboClaw::comando47(char address)
+{
+    char cmd = 47, recebe[7], chegar = 0, checksum_recebido = 0, checksum = 0;
+    unsigned int i = 0;
+    unsigned short dist = 0, aux = 0;  
+
+    Robo.baud(38400);
+
+
+    envia_8(address);
+    envia_8(cmd);
+    
+    for(i=1 ; i<=3 ; i++)
+    {
+        chegar = Robo.getc();
+        recebe[i] = chegar; 
+    }
+    
+    aux = recebe[1];
+    aux = (aux<<8);
+    dist = dist + aux;
+    aux = recebe[2];     
+    dist = dist + aux;
+
+
+    checksum_recebido = (recebe[3] - 0x2F)&0x7F; // Tem que subtrair erro da placa de controle 
+
+    checksum = 0;
+    for(i=1 ; i<=2 ; i++)
+    {
+        chegar = recebe[i]; 
+        checksum = checksum + chegar;
+    }
+    checksum = checksum&0x7F;
+
+    if(checksum == checksum_recebido)
+        return (dist);
+    else
+        return (0);
+}
+        
+// Funcoes Sendor de Distância Sharp:
+double RoboClaw::sharp(char y)
+{
+    double resultado = 0, x = 0;
+    
+    x = le_32_vezes(y);
+
+    // Grafico 2: 
+    //y = 2,266x6 - 25,396x5 + 114,73x4 - 268,77x3 + 349,89x2 - 251,81x + 92,04
+    //y = 2,2659705249x6 - 25,3957269119x5 + 114,7331566401x4 - 268,7667878147x3 + 349,8876615694x2 - 251,8100630442x + 92,0400793302
+
+    if(x <= 0.4)
+        return (-2); // Erro: Distância maior que 30cm
+    if(x >= 2.8)
+        return (-1); // Erro: Distância menor que 4cm
+
+    resultado =2.2659705249*x*x*x*x*x*x;
+    resultado = resultado - 25.3957269119*x*x*x*x*x;
+    resultado = resultado + 114.7331566401*x*x*x*x;
+    resultado = resultado - 268.7667878147*x*x*x;
+    resultado = resultado + 349.8876615694*x*x;
+    resultado = resultado - 251.8100630442*x;
+    resultado = resultado + 92.0400793302;
+
+    if(resultado > 30)
+        return (-2); // Erro: Distância maior que 30cm
+    if(resultado < 4)
+        return (-1); // Erro: Distância menor que 4cm
+    
+    return (resultado);
+}

--- a/RoboClaw.h	Sun Sep 16 15:07:33 2012 +0000
+++ b/RoboClaw.h	Wed Nov 21 17:18:43 2012 +0000
@@ -1,17 +1,47 @@
-/* mbed TextLCD Library, for RoboClaw 15A
- */
-
-#ifndef MBED_ROBOCLAW_H
-#define MBED_ROBOCLAW_H
-
-#include "mbed.h"
-
-class RoboClaw{
-public:
-    RoboClaw(void);
-    void comando46 (char address, unsigned int accel, int qspeedM1, unsigned int distanceM1, int qspeedM2, unsigned int distanceM2, char buffer);
-private:
-}
-;
-
-#endif
+/* mbed TextLCD Library, for RoboClaw 15A
+ */
+
+#ifndef MBED_ROBOCLAW_H
+#define MBED_ROBOCLAW_H
+
+#include "mbed.h"
+
+class RoboClaw {
+public:
+    RoboClaw(void);
+    void comando0(char address, char value);
+    void comando1(char address, char value);
+    void comando2(char address, char value);
+    void comando3(char address, char value); 
+    void comando4(char address, char value);
+    void comando5(char address, char value);
+    void comando8(char address, char value);
+    void comando9(char address, char value);
+    void comando10(char address, char value);
+    void comando11(char address, char value);
+    void comando13(char address, char value);
+    unsigned long long comando16(char address);
+    unsigned long long comando17(char address);
+    unsigned long long comando18(char address);
+    unsigned long long comando19(char address);
+    void comando20(char address);
+    void comando28(char address, unsigned int D, unsigned int P, unsigned int I, unsigned int QPPS);
+    void comando29(char address, unsigned int D, unsigned int P, unsigned int I, unsigned int QPPS);
+    void comando35(char address, int qspeedM1);
+    void comando36(char address, int qspeedM2);
+    void comando37(char address, int qspeedM1, int qspeedM2);
+    void comando38(char address, unsigned int accel, int qspeedM1);
+    void comando39(char address, unsigned int accel, int qspeedM2);
+    void comando40(char address, unsigned int accel, int qspeedM1, int qspeedM2);    
+    void comando41(char address, int qspeedM1, unsigned int distanceM1, char cbuffer);
+    void comando42(char address, int qspeedM2, unsigned int distanceM2, char cbuffer);
+    void comando43(char address, int qspeedM1, unsigned int distanceM1, int qspeedM2, unsigned int distanceM2, char cbuffer);
+    void comando44(char address, unsigned int accel, int qspeedM1, unsigned int distanceM1, char cbuffer);
+    void comando45(char address, unsigned int accel, int qspeedM2, unsigned int distanceM2, char cbuffer);
+    void comando46(char address, unsigned int accel, int qspeedM1, unsigned int distanceM1, int qspeedM2, unsigned int distanceM2, char cbuffer);
+    unsigned short comando47(char address);
+    double sharp(char y);
+}
+;
+
+#endif