Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Can now read Response Packets.

Signed-off-by: Cruz Monrreal II <Cruz.Monrreal@gmail.com>
  • Loading branch information...
commit f8c4e45c8b1d61b0e453d278951c0a571760dfea 1 parent 1f3a5b6
@cmonr authored
Showing with 77 additions and 55 deletions.
  1. +60 −42 Sphero.cpp
  2. +17 −13 Sphero.h
View
102 Sphero.cpp
@@ -6,75 +6,93 @@
https://github.com/cmonr/Arduino-Bluetooth-Library
************************************************/
-//
-// Includes
-//
-
#include <stdint.h>
#include <Arduino.h>
#include "Sphero.h"
-//
-// Constructor
-//
Sphero::Sphero(SoftwareSerial &serial){
bluetooth = &serial;
}
-//
-// Destructor
-//
Sphero::~Sphero(){}
-
//uint8_t Sphero::boost(uint16_t heading, uint8_t duration);
-uint8_t Sphero::roll(short heading, char speed){
- return sendSynchronousPacket(0x02, 0x30, 0x02, 0x05, speed, heading >> 8 , heading, 0xFF);
+void Sphero::roll(short heading, char speed){
+ sendSynchronousPacket(0x02, 0x30, 0x02, 0x05, speed, heading >> 8 , heading, 0xFF);
+}
+//void Sphero::stop();
+void Sphero::setRGBColor(char red, char green, char blue){
+ sendSynchronousPacket(0x02, 0x20, 0x01, 0x05, red, green, blue, 0xFF);
}
-//uint8_t Sphero::stop();
-uint8_t Sphero::setRGBColor(char red, char green, char blue){
- return sendSynchronousPacket(0x02, 0x20, 0x01, 0x05, red, green, blue, 0x00);
+void Sphero::getRGBColor(){
+ sendSynchronousPacket(0x02, 0x22, 0x06, 0x01);
}
-uint8_t Sphero::setBackLED(char intensity){
- return sendSynchronousPacket(0x02, 0x21, 0x04, 0x02, intensity);
+void Sphero::setBackLED(char intensity){
+ sendSynchronousPacket(0x02, 0x21, 0x04, 0x02, intensity);
}
-//uint8_t Sphero::setRotationRate(uint8_t rate);
-uint8_t Sphero::rotateHeadingBy(short heading){
- return sendSynchronousPacket(0x02, 0x01, 0x03, 0x03, heading >> 8 , heading);
+//void Sphero::setRotationRate(uint8_t rate);
+void Sphero::rotateHeadingBy(short heading){
+ sendSynchronousPacket(0x02, 0x01, 0x03, 0x03, heading >> 8 , heading);
}
-//uint8_t Sphero::setMotionTimeout();
-//uint8_t Sphero::setRawMotorValues(uint8_t l_mode, uint8_t r_mode, uint8_t l_pwr, uint8_t r_pwr);
+//void Sphero::setMotionTimeout();
+//void Sphero::setRawMotorValues(uint8_t l_mode, uint8_t r_mode, uint8_t l_pwr, uint8_t r_pwr);
-uint8_t Sphero::sendSynchronousPacket(uint8_t DID, uint8_t CID, uint8_t SEQ, uint8_t DLEN, ...){
- uint16_t i=0;
- uint8_t sum=0, tmp;
+void Sphero::sendSynchronousPacket(char DID, char CID, char SEQ, char DLEN, ...){
+ short i=0;
+ char sum=0, data;
va_list args;
- bluetooth->write(uint8_t(0xFF));
- bluetooth->write(uint8_t(0xFF));
- bluetooth->write(uint8_t(DID));
- bluetooth->write(uint8_t(CID));
- bluetooth->write(uint8_t(SEQ));
- bluetooth->write(uint8_t(DLEN));
+ // Write data
+ bluetooth->write(char(0xFF));
+ bluetooth->write(char(0xFF));
+ bluetooth->write(char(DID));
+ bluetooth->write(char(CID));
+ bluetooth->write(char(SEQ));
+ bluetooth->write(char(DLEN));
+ // Calculate checksum
sum += DID + CID + SEQ + DLEN;
va_start(args, DLEN);
for(; i<DLEN-1; i++){
- tmp = va_arg(args, int);
- bluetooth->write(uint8_t(tmp));
- sum += tmp;
+ data = va_arg(args, int);
+ bluetooth->write(char(data));
+ sum += data;
}
va_end(args);
- //delay(1);
- bluetooth->write(uint8_t(~sum));
- //bluetooth->print('\n');
- return readResponsePacket();
+ // Write Checksum
+ bluetooth->write(char(~sum));
+
+ // Wait for response
+ readResponsePacket();
}
-uint8_t Sphero::readResponsePacket(){
- //while(bluetooth->available());
+void Sphero::readResponsePacket(){
+ char ctr = 0;
+ response = "";
+
+ bluetooth->flush(); // Get rid of any stale data
+
+ while(!bluetooth->available()){} // Wait for response packet to begin
- return 0;
+ response += char(bluetooth->read()); // Append first character
+
+ while(ctr < 32 ){ // Arbritrary value
+ if (bluetooth->available()){
+ response += char(bluetooth->read());
+ ctr = 0;
+ }else{
+ ctr++;
+ }
+ }
+}
+
+String Sphero::readRawResponse(){ return response; }
+
+String Sphero::readResponseData(){
+ if (response.charAt(4) > 1)
+ return response.substring(5, 5 + (response.charAt(4)-1));
+ return "";
}
+
View
30 Sphero.h
@@ -20,24 +20,28 @@ class Sphero{
~Sphero();
// Set
- //uint8_t boost(uint16_t heading, uint8_t duration);
- uint8_t roll( short heading, char speed );
- //uint8_t stop();
- uint8_t setRGBColor( char red, char green, char blue );
- uint8_t setBackLED( char intensity);
- //uint8_t setRotationRate(uint8_t rate);
- uint8_t rotateHeadingBy( short heading );
- //uint8_t setMotionTimeout();
- //uint8_t setRawMotorValues(uint8_t l_mode, uint8_t r_mode, uint8_t l_pwr, uint8_t r_pwr); // Special note. See Datasheet.
+ //void boost(uint16_t heading, uint8_t duration);
+ void roll( short heading, char speed );
+ //void stop();
+ void setRGBColor( char red, char green, char blue );
+ void getRGBColor( void );
+ void setBackLED( char intensity);
+ //void setRotationRate(uint8_t rate);
+ void rotateHeadingBy( short heading );
+ //void setMotionTimeout();
+ //void setRawMotorValues(uint8_t l_mode, uint8_t r_mode, uint8_t l_pwr, uint8_t r_pwr); // Special note. See Datasheet.
// Get
- //uint8_t getHeading(uint16_t* data);
+ //void getHeading(uint16_t* data);
- uint8_t readResponsePacket();
+
+ String readRawResponse( void );
+ String readResponseData( void );
private:
- uint8_t sendSynchronousPacket(uint8_t DID, uint8_t CID, uint8_t SEQ, uint8_t DLEN, ...);
+ void sendSynchronousPacket( char DID, char CID, char SEQ, char DLEN, ... );
+ void readResponsePacket( void );
SoftwareSerial *bluetooth;
- unsigned char response_buff[32];
+ String response;
};
#endif
Please sign in to comment.
Something went wrong with that request. Please try again.