├── .gitignore ├── README.md ├── Sphero.cpp ├── Sphero.h ├── examples ├── AutoConfigBluetoothForSphero │ └── AutoConfigBluetoothForSphero.ino ├── ReadIMU │ └── ReadIMU.ino ├── SpheroTankControl │ └── SpheroTankControl.ino ├── SpinMeRightRound │ └── SpinMeRightRound.ino ├── SquareRoll │ └── SquareRoll.ino └── YawToHue │ └── YawToHue.ino └── keywords.txt /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | 9 | # Compiled Static libraries 10 | *.lai 11 | *.la 12 | *.a 13 | 14 | # Notepad++ Autobackup 15 | *.bak -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Arduino-Sphero-Library 2 | ====================== 3 | 4 | License: https://creativecommons.org/licenses/by/4.0/ 5 | 6 | 7 | Notice: At this time, I do not plan on updating the Arduino Sphero or Bluetooth libraries. 8 | 9 | 10 | This is a -basic- Sphero Library for the Arduino. 11 | 12 | 13 | Things needed: 14 | 15 | * Bluetooth Modem (http://www.sparkfun.com/products/9358) 16 | * Arduino Mega 2560 17 | * Jumper Wires 18 | 19 | 20 | Instructions: 21 | 22 | 1. Connect tx,rx pins on Bluetooth Module to TX1, RX1 on Arduino Mega 23 | 2. Connect CTS and RTS to eachother 24 | 3. Connect 5v and GND to Arduino 5V and GND 25 | 4. Shake on Sphero 26 | 5. Upload the AutoConfigBluetoothForSphero example, and run with the Serial Monitor 27 | 6. Power cycle Arduino & Bluetooth Module (unplug & replug USB cable) 28 | : NOTE: Upon power up, the Sphero should autoconnect 29 | 7. Upload and run any of the examples 30 | -------------------------------------------------------------------------------- /Sphero.cpp: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 06-26-2012 4 | 5 | Updates can be found here: 6 | https://github.com/cmonr/Arduino-Bluetooth-Library 7 | ************************************************/ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | Sphero::Sphero(){ 14 | Serial1.begin(115200); 15 | 16 | streamingParams.numOfPackets = 16; 17 | } 18 | 19 | Sphero::~Sphero(){} 20 | 21 | //uint8_t Sphero::boost(uint16_t heading, uint8_t duration); 22 | char Sphero::roll(short heading, char speed){ 23 | return sendCommand(0x02, 0x30, 0x02, 0x05, speed, heading >> 8 , heading, 0xFF); 24 | } 25 | char Sphero::setHeading(short heading){ 26 | return sendCommand(0x02, 0x01, 0x0D, 0x03, heading >> 8, heading); 27 | } 28 | char Sphero::setRGBColor(char red, char green, char blue){ 29 | return sendCommand(0x02, 0x20, 0x01, 0x05, red, green, blue, 0xFF); 30 | } 31 | char Sphero::getRGBColor(){ 32 | return sendCommand(0x02, 0x22, 0x06, 0x00); 33 | } 34 | char Sphero::setBackLED(char intensity){ 35 | return sendCommand(0x02, 0x21, 0x04, 0x02, intensity); 36 | } 37 | //void Sphero::setRotationRate(uint8_t rate); 38 | char Sphero::rotateHeadingBy(short heading){ 39 | return sendCommand(0x02, 0x01, 0x03, 0x03, heading >> 8 , heading); 40 | } 41 | //void Sphero::setMotionTimeout(); 42 | 43 | char Sphero::setMotorPowers(signed short p1, signed short p2){ 44 | char dir1 = 0x01, 45 | dir2 = 0x01, 46 | m1 = p1, 47 | m2 = p2; 48 | 49 | if (p1 < 0){ 50 | m1 = p1 * -1; 51 | dir1 = 0x02; 52 | } 53 | 54 | if (p2 < 0){ 55 | m2 = p2 * -1; 56 | dir2 = 0x02; 57 | } 58 | 59 | return sendCommand(0x02, 0x33, 0x0B, 0x05, dir1, m1, dir2, m2); 60 | } 61 | char Sphero::stop(char coast){ 62 | return sendCommand(0x02, 0x33, 0x0C, 0x05, coast ? 0x03 : 0x00, 0x00, coast ? 0x03 : 0x00, 0x00); 63 | } 64 | 65 | 66 | char Sphero::setStabilization(char enable){ 67 | return sendCommand(0x02, 0x33, 0x0A, 0x05, 0x04, 0x00, 0x04, 0x00)<<4 | sendCommand(0x02, 0x02, 0x09, 0x02, enable ? 0x01 : 0x00); 68 | } 69 | 70 | char Sphero::setStreamingData(short freq, short frames_per_sample, long int mask){ 71 | streamingParams.count = 0; 72 | streamingParams.freq = freq; 73 | streamingParams.frames_per_sample = frames_per_sample; 74 | streamingParams.mask = mask; 75 | 76 | return sendCommand(0x02, 0x11, 0x07, 0x0A, (400/freq) >> 8, (400/freq), frames_per_sample >> 8, frames_per_sample, char(mask >> 24), char(mask >> 16), char(mask >> 8), char(mask), streamingParams.numOfPackets); 77 | } 78 | 79 | char Sphero::getOptionFlags(){ 80 | return sendCommand(0x02, 0x36, 0x08, 0x01); 81 | } 82 | 83 | char Sphero::sendCommand(char DID, char CID, char SEQ, char DLEN, ...){ 84 | short i=0; 85 | char sum=0, data; 86 | va_list args; 87 | 88 | // Flush pipe (just in case) 89 | //Serial1.flush(); 90 | 91 | // Write data 92 | Serial1.write(char(0xFF)); 93 | Serial1.write(char(0xFF)); 94 | Serial1.write(char(DID)); 95 | Serial1.write(char(CID)); 96 | Serial1.write(char(SEQ)); 97 | Serial1.write(char(DLEN)); 98 | 99 | // Calculate checksum 100 | sum += DID + CID + SEQ + DLEN; 101 | 102 | va_start(args, DLEN); 103 | for(; i 32) // Error 155 | return -1; 156 | 157 | for(i=0; i 32) // Error 186 | return; 187 | 188 | for(i=0; i 13 | #include 14 | #include 15 | 16 | #define SPHERO_IMU_YAW 0x00010000 17 | #define SPHERO_IMU_ROLL 0x00020000 18 | #define SPHERO_IMU_PITCH 0x00040000 19 | 20 | class Sphero{ 21 | 22 | public: 23 | // public methods 24 | Sphero(); 25 | ~Sphero(); 26 | 27 | // Set 28 | //void boost(uint16_t heading, uint8_t duration); 29 | char roll( short heading, char speed ); 30 | //void stop(); 31 | char setHeading( short heading ); 32 | char setRGBColor( char red, char green, char blue ); 33 | char getRGBColor( void ); 34 | char setBackLED( char intensity ); 35 | //void setRotationRate(uint8_t rate); 36 | char rotateHeadingBy( short heading ); 37 | char getOptionFlags( void ); 38 | //void setMotionTimeout(); 39 | 40 | 41 | // setRawMotorValues 42 | // *** Disables Stabilization 43 | // *** Will need to be reenabled 44 | char setMotorPowers( signed short m1, signed short m2 ); 45 | char stop( char coast ); 46 | 47 | 48 | // Get 49 | char setStreamingData( short sample_freq_divisor, short frames_per_packet, long int mask ); 50 | char setStabilization( char enable ); 51 | 52 | // Packet attributes 53 | char getMRSP( void ); 54 | char getSequenceNum( void ); 55 | char getID( void ); 56 | char getChecksum( void ); 57 | short getDataLength( void ); 58 | unsigned char getData( char num ); 59 | 60 | void readAsyncPacket( void ); 61 | 62 | private: 63 | char sendCommand( char DID, char CID, char SEQ, char DLEN, ... ); 64 | char readSimplePacket( void ); 65 | 66 | char mrsp; 67 | char seq; 68 | short len; 69 | unsigned char data[32]; 70 | char chksum; 71 | 72 | typedef struct { 73 | char count; 74 | char numOfPackets; 75 | 76 | short freq; 77 | short frames_per_sample; 78 | long mask; 79 | } StreamingParams; 80 | StreamingParams streamingParams; 81 | }; 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /examples/AutoConfigBluetoothForSphero/AutoConfigBluetoothForSphero.ino: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 06-26-2012 4 | MOdified on 10-20-2012 5 | 6 | Updates can be found here: 7 | https://github.com/cmonr/Arduino-Sphero-Library 8 | ************************************************/ 9 | 10 | void setup(){ 11 | // Give user a chance to power cycle Bluetooth Module 12 | delay(5000); 13 | 14 | // Init USB Serial 15 | Serial.begin(115200); // Default Baud Rate 16 | Serial.println("Initializing Bluetooth"); 17 | 18 | // Init Blutooth Serial 19 | // Uncomment to change baud rate 20 | /*Serial1.begin(115200); 21 | sendCmd("$$$"); 22 | sendCmd("U,115200,N"); // *** Replace this with desired Baud Rate *** 23 | sendCmd("---"); 24 | */ 25 | 26 | // Reconnect to Bluetooth and configure to master 27 | Serial1.begin(115200); // *** This too *** 28 | sendCmd("$$$"); 29 | sendCmd("SM,1"); 30 | sendCmd("---"); 31 | 32 | Serial.println("\nEntering Command Mode"); 33 | sendCmd("$$$"); 34 | 35 | Serial.println("\nScanning for Spheros..."); 36 | sendCmd("I,10"); 37 | } 38 | 39 | void loop(){ 40 | long start; 41 | String line; 42 | String sphero_id=""; 43 | boolean commandComplete; 44 | 45 | line = ""; 46 | commandComplete = false; 47 | while(!commandComplete){ 48 | while(Serial1.available()){ 49 | unsigned char tmp = Serial1.read(); 50 | if (tmp == '\n'){ // End of Line 51 | if (line.indexOf("Sphero") != -1){ 52 | //Save result 53 | //We can't exit because we need to wait for the command to finish 54 | if (sphero_id == ""){ 55 | sphero_id = line; 56 | Serial.println(line); 57 | } 58 | } 59 | 60 | if (line.indexOf("Done") != -1) 61 | // We can leave now... 62 | commandComplete = true; 63 | 64 | line = ""; 65 | }else 66 | line.concat(char(tmp)); 67 | } 68 | } 69 | 70 | if (sphero_id != ""){ 71 | Serial.print("Connecting to "); 72 | Serial.println(sphero_id.substring(13, sphero_id.indexOf(',', 13))); 73 | 74 | // Connect to Sphero Address 75 | sendCmd("C," + sphero_id.substring(0, 12)); 76 | Serial.println("Connected."); 77 | 78 | // Save address 79 | sendCmd("SR," + sphero_id.substring(0, 12)); 80 | 81 | // We're done here! 82 | sendCmd("---"); 83 | Serial.println("\nConfiguration complete!\nYou're Sphero's address has been saved in the Bluetooth Module\n\nHave fun ^^;"); 84 | 85 | // Idle... 86 | while(true); 87 | } 88 | } 89 | 90 | void sendCmd(String cmd){ 91 | String line = ""; 92 | 93 | // Send command 94 | if (cmd == "$$$") 95 | Serial1.print(cmd); 96 | else 97 | Serial1.println(cmd); 98 | 99 | // Short delay... 100 | delay(100); 101 | 102 | // Show which cmd is being sent 103 | Serial.println("> " + cmd); 104 | 105 | while(1){ 106 | if (Serial1.available()){ 107 | unsigned char tmp = Serial1.read(); 108 | if (tmp == '\n') 109 | break; 110 | else 111 | line.concat(char(tmp)); 112 | } 113 | } 114 | 115 | // Show Response 116 | Serial.println(line); 117 | } -------------------------------------------------------------------------------- /examples/ReadIMU/ReadIMU.ino: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 08-19-2012 4 | Modified on 10/20/2012 5 | 6 | Updates can be found here: 7 | https://github.com/cmonr/Arduino-Sphero-Library 8 | ************************************************/ 9 | 10 | #include 11 | #include 12 | 13 | Sphero sphero; 14 | Bluetooth bluetooth; 15 | 16 | void setup() { 17 | Serial.begin(115200); 18 | 19 | // Connect to Sphero 20 | bluetooth.beginCMD(); 21 | bluetooth.connect(); 22 | bluetooth.endCMD(); 23 | 24 | // Indicator LED 25 | sphero.setBackLED(0xFF); 26 | 27 | // Lock motors 28 | sphero.setStabilization(0); 29 | 30 | // Get Pitch @ 25Hz 31 | sphero.setStreamingData(25, 1, SPHERO_IMU_PITCH | SPHERO_IMU_ROLL | SPHERO_IMU_YAW); 32 | } 33 | 34 | void loop() { 35 | sphero.readAsyncPacket(); 36 | 37 | // Output Pitch, Roll, Yaw 38 | Serial.print((signed short)sphero.getData(0) << 8 | sphero.getData(1)); 39 | Serial.print(", "); 40 | Serial.print((signed short)sphero.getData(2) << 8 | sphero.getData(3)); 41 | Serial.print(", "); 42 | Serial.println((signed short)sphero.getData(4) << 8 | sphero.getData(5)); 43 | } -------------------------------------------------------------------------------- /examples/SpheroTankControl/SpheroTankControl.ino: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 08-19-2012 4 | Modified on 11-11-2012 5 | 6 | Updates can be found here: 7 | https://github.com/cmonr/Arduino-Sphero-Library 8 | ************************************************/ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | Bluetooth bluetooth; 15 | Sphero sphero; 16 | SabertoothSimplified ST; 17 | 18 | void setup() { 19 | Serial.begin(9600); 20 | ST.drive(0); 21 | ST.turn(0); 22 | 23 | delay(3000); 24 | 25 | // Connect to Sphero 26 | bluetooth.beginCMD(); 27 | bluetooth.connect(); 28 | bluetooth.endCMD(); 29 | 30 | // Lock motors 31 | sphero.setStabilization(0); 32 | 33 | // Give user a chance to align w/ dot 34 | for(int i=0; i<10; i++){ 35 | sphero.setBackLED(0xFF); 36 | delay(500); 37 | sphero.setBackLED(0x00); 38 | delay(500); 39 | } 40 | 41 | // Indicator LED 42 | sphero.setBackLED(0xFF); 43 | 44 | // Init Heading to 0 45 | sphero.setHeading(0x00); 46 | 47 | // Get Pitch @ 10Hz 48 | sphero.setStreamingData(10, 1, SPHERO_IMU_PITCH | SPHERO_IMU_YAW); 49 | } 50 | 51 | void loop() { 52 | signed short pitch, yaw; 53 | sphero.readAsyncPacket(); 54 | 55 | pitch = sphero.getData(0) << 8 | sphero.getData(1); 56 | yaw = sphero.getData(2) << 8 | sphero.getData(3); 57 | 58 | if (yaw < -70 && yaw > -140){ 59 | ST.turn(50); 60 | }else if (yaw > 70 && yaw < 140){ 61 | ST.turn(-50); 62 | }else if (pitch < -50){ 63 | ST.drive(-100); 64 | //ST.turn(0); 65 | }else if (pitch > 50){ 66 | ST.drive(100); 67 | }else{ 68 | ST.drive(0); 69 | ST.turn(0); 70 | } 71 | } -------------------------------------------------------------------------------- /examples/SpinMeRightRound/SpinMeRightRound.ino: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 06-26-2012 4 | Modified on 10-20-2012 5 | 6 | Updates can be found here: 7 | https://github.com/cmonr/Arduino-Sphero-Library 8 | ************************************************/ 9 | 10 | #include 11 | Sphero sphero; 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | 16 | // Turn on Back LED 17 | sphero.setBackLED(0xFF); 18 | 19 | // Turn off Stabilization 20 | sphero.setStabilization(0); 21 | 22 | // ??? You Spin Me Right Round ??? 23 | sphero.setMotorPowers(-80, 80); 24 | } 25 | 26 | void loop(){} 27 | -------------------------------------------------------------------------------- /examples/SquareRoll/SquareRoll.ino: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 10-20-2012 4 | 5 | Updates can be found here: 6 | https://github.com/cmonr/Arduino-Bluetooth-Library 7 | ************************************************/ 8 | 9 | #include 10 | 11 | Sphero sphero; 12 | 13 | void setup() { 14 | // Turn on Back LED 15 | sphero.setBackLED(0xFF); 16 | } 17 | 18 | void loop() { 19 | sphero.roll(0, 0x80); 20 | delay(1000); 21 | 22 | sphero.stop(0, 0x00); 23 | delay(250); 24 | 25 | sphero.roll(90, 0x80); 26 | delay(1000); 27 | 28 | sphero.stop(0, 0x00); 29 | delay(250); 30 | 31 | sphero.roll(180, 0x80); 32 | delay(1000); 33 | 34 | sphero.stop(0, 0x00); 35 | delay(250); 36 | 37 | sphero.roll(270, 0x80); 38 | delay(1000); 39 | 40 | sphero.stop(0, 0x00); 41 | delay(250); 42 | } -------------------------------------------------------------------------------- /examples/YawToHue/YawToHue.ino: -------------------------------------------------------------------------------- 1 | /************************************************ 2 | Written by Cruz Monrreal II 3 | Created on 10-20-2012 4 | 5 | Initial HSV-to-RGB code from 6 | http://splinter.com.au/blog/?p=29 7 | 8 | Updates can be found here: 9 | https://github.com/cmonr/Arduino-Sphero-Library 10 | ************************************************/ 11 | 12 | #include 13 | Sphero sphero; 14 | 15 | void setup() { 16 | // Turn on Back LED 17 | sphero.setBackLED(0xFF); 18 | 19 | // Turn off Stabilization 20 | sphero.setStabilization(0); 21 | 22 | // Init current heading to 0 23 | sphero.setHeading(0x00); 24 | 25 | // Setup streaming data of Yaw angle 26 | sphero.setStreamingData(10, 1, SPHERO_IMU_YAW); 27 | } 28 | 29 | void loop(){ 30 | // Busy-wait until Async packet is received 31 | sphero.readAsyncPacket(); 32 | 33 | // Convert Angle to RGB and set Sphero's Color accordingly 34 | setLedColorHSV((signed short)(sphero.getData(0) << 8 | sphero.getData(1)) + 180); 35 | } 36 | 37 | void setLedColorHSV(int h) { 38 | float r,g,b; 39 | r=g=b=0; 40 | 41 | float hf=h/60.0; 42 | 43 | char i=(char) floor(h/60.0); 44 | float f = h/60.0 - i; 45 | float qv = 1 - f; 46 | float tv = 1 - (1 - f); 47 | 48 | switch (i){ 49 | case 0: r = 1; g = tv; b = 0; break; 50 | case 1: r = qv; g = 1; b = 0; break; 51 | case 2: r = 0; g = 1; b = tv; break; 52 | case 3: r = 0; g = qv; b = 1; break; 53 | case 4: r = tv; g = 0; b = 1; break; 54 | case 5: r = 1; g = 0; b = qv; break; 55 | } 56 | 57 | sphero.setRGBColor(r*255, g*255, b*255); 58 | } 59 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map for Sphero 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | Sphero KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | roll KEYWORD2 16 | setRGBColor KEYWORD2 17 | setBackLED KEYWORD2 18 | rotateHeadingBy KEYWORD2 19 | 20 | ####################################### 21 | # Constants (LITERAL1) 22 | ####################################### 23 | 24 | --------------------------------------------------------------------------------