diff --git a/README.md b/README.md index 818bd6c..ae77f45 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,42 @@ -HelloSpoon-Spark -============= +Dynamixel XL-320 +================ -**Work in progress... not ready yet! Really. +Dynamixel XL-320 servo library for Adruino -This is a library created to control HelloSpoon robot DIY version. -HelloSpoon is a mealtime companion robot intended to feed children and elderly -with upper limb difficulties while giving them an entertaining time. +## A Servo library for Arduino -HelloSpoon hardware is based on: +Clone this repository into your Arduino libraries folder: -Spark Core - http://docs.spark.io/hardware/ +``` $ cd ~/Documents/Arduino/libraries/ ``` -DYNAMIXEL XL-320 servos - http://support.robotis.com/en/product/dynamixel/xl-320/xl-320.htm +Restart Arduino app. :-) -For wiring, programming tutorials and more, please go to the Wiki section of this page. +Open the example Arduino sketch to see how it works: -Code based on: +``` File > Examples > XL320 > XL320_servo_example ``` -Ax-12+ Half Duplex USART Comunication Copyright (c) 2011 Savage Electronics -and Dynamixel Pro library for OpenCM-9.04 made by ROBOTIS, LTD. +### Hardware -Modifications made by Luis G III for HelloSpoon robot. +[DYNAMIXEL XL-320 servo manual](http://support.robotis.com/en/product/dynamixel/xl-320/xl-320.htm) including specifications and message sending addresses. -Webpage - http://hellospoonrobot.com -Twitter - @HelloSpoon -Youtube - http://youtube.com/user/hellospoonrobot +### Wiring diagram + +Looking from above, with the servo head at the top, wire the left plug of the servo to: + +* PIN1: GND +* PIN2: 5 volts +* PIN3: Serial TX + +![Dynamixel XL-320 wiring diagram](XL320-wiring.jpg) + +### Setting the servo serial baud rate & servoID + +We've included some example sketches to help test and setup your servos. Out of the box they're set to communicate via serial at 1Mbps, so you might want to set them down to something more managable by Arduino at 115200. + +Follow the instructions in the sketch ```XL320_servo_set_baud_rate_or_id.ino``` and don't forget to power cycle the servos in between setting anything. + +Note: when setting the ServoID, the servos default down to 9600 baud, so after you set the servoID you'll need to set the baud rate back up to 115200. + +### More information + +Read more about this library on the [Hackerspace Adelaide Wiki](http://hackerspace-adelaide.org.au/wiki/Dynamixel_XL-320) diff --git a/XL320-arduino-library.jpg b/XL320-arduino-library.jpg new file mode 100644 index 0000000..626e4ec Binary files /dev/null and b/XL320-arduino-library.jpg differ diff --git a/XL320-wiring.jpg b/XL320-wiring.jpg new file mode 100644 index 0000000..6b12ea4 Binary files /dev/null and b/XL320-wiring.jpg differ diff --git a/XL320.cpp b/XL320.cpp new file mode 100644 index 0000000..07d35fc --- /dev/null +++ b/XL320.cpp @@ -0,0 +1,487 @@ +/* + + Code based on: + Dynamixel.cpp - Ax-12+ Half Duplex USART Comunication + Copyright (c) 2011 Savage Electronics. + And Dynamixel Pro library for OpenCM-9.04 made by ROBOTIS, LTD. + + Modified to work only with Dynamixel XL-320 actuator. + + Modifications made by Luis G III for XL320 robot. + Webpage: http://hellospoonrobot.com + Twitter: @XL320 + Youtube: http://youtube.com/user/hellospoonrobot + + This file can be used and be modified by anyone, + don't forget to say thank you to OP! + + */ + +#include "Arduino.h" +#include "dxl_pro.h" +#include "XL320.h" +#include +#include + +// Macro for the selection of the Serial Port +#define sendData(args) (this->stream->write(args)) // Write Over Serial +#define beginCom(args) // Begin Serial Comunication +#define readData() (this->stream->read()) + +// Select the Switch to TX/RX Mode Pin +#define setDPin(DirPin,Mode) +#define switchCom(DirPin,Mode) // Switch to TX/RX Mode + +#define NANO_TIME_DELAY 12000 + + +XL320::XL320() { + +} + +XL320::~XL320() { +} + +void XL320::begin(Stream &stream) +{ + //setDPin(Direction_Pin=4,OUTPUT); + //beginCom(1000000); + this->stream = &stream; +} + +void XL320::moveJoint(int id, int value){ + int Address = XL_GOAL_POSITION_L; + + sendPacket(id, Address, value); + this->stream->flush(); + + nDelay(NANO_TIME_DELAY); +} + +void XL320::setJointSpeed(int id, int value){ + int Address = XL_GOAL_SPEED_L; + sendPacket(id, Address, value); + this->stream->flush(); + + nDelay(NANO_TIME_DELAY); + +} + +void XL320::LED(int id, char led_color[]){ + int Address = XL_LED; + int val = 0; + + if(led_color[0] == 'r'){ + val = 1; + } + + else if(led_color[0] == 'g'){ + val = 2; + } + + else if(led_color[0] == 'y'){ + val = 3; + } + + else if(led_color[0] == 'b'){ + val = 4; + } + + else if(led_color[0] == 'p'){ + val = 5; + } + + else if(led_color[0] == 'c'){ + val = 6; + } + + else if(led_color[0] == 'w'){ + val = 7; + } + + else if(led_color[0] == 'o'){ + val = 0; + } + + sendPacket(id, Address, val); + this->stream->flush(); + + nDelay(NANO_TIME_DELAY); +} + +void XL320::setJointTorque(int id, int value){ + int Address = XL_GOAL_TORQUE; + sendPacket(id, Address, value); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); + +} + +void XL320::TorqueON(int id){ + + int Address = XL_TORQUE_ENABLE; + int value = 1; + + sendPacket(id, Address, value); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); +} + +void XL320::TorqueOFF(int id){ + + int Address = XL_TORQUE_ENABLE; + int value = 0; + + sendPacket(id, Address, value); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); +} + + +void XL320::quickTest(){ + + int position_tmp = 0; + + for(int id = 1; id < 6; id++){ + sendPacket(id, XL_LED, random(1,7)); + nDelay(NANO_TIME_DELAY); + this->stream->flush(); + sendPacket(id, XL_GOAL_SPEED_L, 200); + nDelay(NANO_TIME_DELAY); + this->stream->flush(); + } + + for(int id = 1; id < 6; id++){ + + position_tmp = random(0,512); + + if(id != 3){ + sendPacket(id, XL_GOAL_POSITION_L, position_tmp); + delay(1000); + this->stream->flush(); + } + + else{ + sendPacket(3, XL_GOAL_POSITION_L, 512-position_tmp); + delay(1000); + this->stream->flush(); + } + } + + for(int id = 1; id < 6; id++){ + sendPacket(id, XL_LED, 2); + nDelay(NANO_TIME_DELAY); + this->stream->flush(); + sendPacket(id, XL_GOAL_SPEED_L, 1023); + nDelay(NANO_TIME_DELAY); + this->stream->flush(); + } + + for(int id = 1; id < 6; id++){ + sendPacket(id, XL_LED, 0); + nDelay(NANO_TIME_DELAY); + this->stream->flush(); + } + +} + +int XL320::getSpoonLoad(){ + int spoon = RXsendPacket(5, XL_PRESENT_LOAD); + this->stream->flush(); + return spoon; +} + +int XL320::getJointPosition(int id){ + unsigned char buffer[255]; + RXsendPacket(id, XL_PRESENT_POSITION, 2); + this->stream->flush(); + if(this->readPacket(buffer,255)>0) { + Packet p(buffer,255); + if(p.isValid() && p.getParameterCount()>=3) { + return (p.getParameter(1))|(p.getParameter(2)<<8); + } else { + return -1; + } + } + return -2; +} + +int XL320::getJointSpeed(int id){ + int speed = RXsendPacket(id, XL_PRESENT_SPEED); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); + return speed; +} + +int XL320::getJointLoad(int id){ + int load = RXsendPacket(id, XL_PRESENT_LOAD); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); + return load; +} + +int XL320::getJointTemperature(int id){ + int temp = RXsendPacket(id, XL_PRESENT_TEMPERATURE); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); + return temp; +} + +int XL320::isJointMoving(int id){ + int motion = RXsendPacket(id, XL_MOVING); + this->stream->flush(); + nDelay(NANO_TIME_DELAY); + return motion; +} + +int XL320::sendPacket(int id, int Address, int value){ + + /*Dynamixel 2.0 communication protocol + used by Dynamixel XL-320 and Dynamixel PRO only. + */ + + // technically i think we need 14bytes for this packet + + const int bufsize = 16; + + byte txbuffer[bufsize]; + + Packet p(txbuffer,bufsize,id,0x03,4, + DXL_LOBYTE(Address), + DXL_HIBYTE(Address), + DXL_LOBYTE(value), + DXL_HIBYTE(value)); + + + int size = p.getSize(); + stream->write(txbuffer,size); + + //stream->write(txbuffer,bufsize); + + return bufsize; +} + + + +void XL320::nDelay(uint32_t nTime){ + /* + uint32_t max; + for( max=0; max < nTime; max++){ + + } + */ +} + +int XL320::flush() { + this->stream->flush(); +} + +int XL320::RXsendPacket(int id, int Address) { + return this->RXsendPacket(id, Address, 2); +} + +int XL320::RXsendPacket(int id, int Address, int size){ + + /*Dynamixel 2.0 communication protocol + used by Dynamixel XL-320 and Dynamixel PRO only. + */ + + const int bufsize = 16; + + byte txbuffer[bufsize]; + + Packet p(txbuffer,bufsize,id,0x02,4, + DXL_LOBYTE(Address), + DXL_HIBYTE(Address), + DXL_LOBYTE(size), + DXL_HIBYTE(size)); + + + stream->write(txbuffer,p.getSize()); + + //stream->write(txbuffer,bufsize); + + return p.getSize(); +} + +// from http://stackoverflow.com/a/133363/195061 + +#define FSM +#define STATE(x) s_##x : if(!stream->readBytes(&BUFFER[I++],1)) goto sx_timeout ; if(I>=SIZE) goto sx_overflow; sn_##x : +#define THISBYTE (BUFFER[I-1]) +#define NEXTSTATE(x) goto s_##x +#define NEXTSTATE_NR(x) goto sn_##x +#define OVERFLOW sx_overflow : +#define TIMEOUT sx_timeout : + +int XL320::readPacket(unsigned char *BUFFER, size_t SIZE) { + int C; + int I = 0; + + int length = 0; + + // state names normally name the last parsed symbol + + + FSM { + STATE(start) { + if(THISBYTE==0xFF) NEXTSTATE(header_ff_1); + I=0; NEXTSTATE(start); + } + STATE(header_ff_1) { + if(THISBYTE==0xFF) NEXTSTATE(header_ff_2); + I=0; NEXTSTATE(start); + } + STATE(header_ff_2) { + if(THISBYTE==0xFD) NEXTSTATE(header_fd); + // yet more 0xFF's? stay in this state + if(THISBYTE==0xFF) NEXTSTATE(header_ff_2); + // anything else? restart + I=0; NEXTSTATE(start); + } + STATE(header_fd) { + // reading reserved, could be anything in theory, normally 0 + } + STATE(header_reserved) { + // id = THISBYTE + } + STATE(id) { + length = THISBYTE; + } + STATE(length_1) { + length += THISBYTE<<8; // eg: length=4 + } + STATE(length_2) { + } + STATE(instr) { + // instr = THISBYTE + // check length because + // action and reboot commands have no parameters + if(I-length>=5) NEXTSTATE(checksum_1); + } + STATE(params) { + // check length and maybe skip to checksum + if(I-length>=5) NEXTSTATE(checksum_1); + // or keep reading params + NEXTSTATE(params); + } + STATE(checksum_1) { + } + STATE(checksum_2) { + // done + return I; + } + OVERFLOW { + return -1; + } + TIMEOUT { + return -2; + } + + } +} + + +XL320::Packet::Packet( + unsigned char *data, + size_t data_size, + unsigned char id, + unsigned char instruction, + int parameter_data_size, + ...) { + + + // [ff][ff][fd][00][id][len1][len2] { [instr][params(parameter_data_size)][crc1][crc2] } + unsigned int length=3+parameter_data_size; + if(!data) { + // [ff][ff][fd][00][id][len1][len2] { [data(length)] } + this->data_size = 7+length; + this->data = (unsigned char*)malloc(data_size); + this->freeData = true; + } else { + this->data = data; + this->data_size = data_size; + this->freeData = false; + } + this->data[0]=0xFF; + this->data[1]=0xFF; + this->data[2]=0xFD; + this->data[3]=0x00; + this->data[4]=id; + this->data[5]=length&0xff; + this->data[6]=(length>>8)&0xff; + this->data[7]=instruction; + va_list args; + va_start(args, parameter_data_size); + for(int i=0;idata[8+i]=arg; + } + unsigned short crc = update_crc(0,this->data,this->getSize()-2); + this->data[8+parameter_data_size]=crc&0xff; + this->data[9+parameter_data_size]=(crc>>8)&0xff; + va_end(args); +} + +XL320::Packet::Packet(unsigned char *data, size_t size) { + this->data = data; + this->data_size = size; + this->freeData = false; +} + + +XL320::Packet::~Packet() { + if(this->freeData==true) { + free(this->data); + } +} + +void XL320::Packet::toStream(Stream &stream) { + stream.print("id: "); + stream.println(this->getId(),DEC); + stream.print("length: "); + stream.println(this->getLength(),DEC); + stream.print("instruction: "); + stream.println(this->getInstruction(),HEX); + stream.print("parameter count: "); + stream.println(this->getParameterCount(), DEC); + for(int i=0;igetParameterCount(); i++) { + stream.print(this->getParameter(i),HEX); + if(igetParameterCount()-1) { + stream.print(","); + } + } + stream.println(); + stream.print("valid: "); + stream.println(this->isValid()?"yes":"no"); +} + +unsigned char XL320::Packet::getId() { + return data[4]; +} + +int XL320::Packet::getLength() { + return data[5]+((data[6]&0xff)<<8); +} + +int XL320::Packet::getSize() { + return getLength()+7; +} + +int XL320::Packet::getParameterCount() { + return getLength()-3; +} + +unsigned char XL320::Packet::getInstruction() { + return data[7]; +} + +unsigned char XL320::Packet::getParameter(int n) { + return data[8+n]; +} + +bool XL320::Packet::isValid() { + int length = getLength(); + unsigned short storedChecksum = data[length+5]+(data[length+6]<<8); + return storedChecksum == update_crc(0,data,length+5); +} diff --git a/firmware/HelloSpoon-Spark.h b/XL320.h similarity index 57% rename from firmware/HelloSpoon-Spark.h rename to XL320.h index 5b31376..8e91b1c 100644 --- a/firmware/HelloSpoon-Spark.h +++ b/XL320.h @@ -16,8 +16,8 @@ */ -#ifndef HELLOSPOON_H_ -#define HELLOSPOON_H_ +#ifndef XL320_H_ +#define XL320_H_ /*EEPROM Area*/ #define XL_MODEL_NUMBER_L 0 @@ -60,38 +60,79 @@ #define Tx_MODE 1 #define Rx_MODE 0 - #include +#include -class HelloSpoon { +class XL320 { private: unsigned char Direction_Pin; volatile char gbpParamEx[130+10]; + Stream *stream; + + void nDelay(uint32_t nTime); + public: - HelloSpoon(); - virtual ~HelloSpoon(); + XL320(); + virtual ~XL320(); - void begin(); + void begin(Stream &stream); - void moveJoint(int Joint, int value); - void setJointSpeed(int Joint, int value); - void LED(int Joint, char led_color[]); - void setJointTorque(int Joint, int value); + void moveJoint(int id, int value); + void setJointSpeed(int id, int value); + void LED(int id, char led_color[]); + void setJointTorque(int id, int value); - void TorqueON(int Joint); - void TorqueOFF(int Joint); - void deactivateTrunk(); - void activateTrunk(); + void TorqueON(int id); + void TorqueOFF(int id); void quickTest(); int getSpoonLoad(); - int getJointPosition(int Joint); - int getJointSpeed(int Joint); - int getJointLoad(int Joint); - int getJointTemperature(int Joint); - int isJointMoving(int Joint); + int getJointPosition(int id); + int getJointSpeed(int id); + int getJointLoad(int id); + int getJointTemperature(int id); + int isJointMoving(int id); + + int sendPacket(int id, int Address, int value); + int readPacket(unsigned char *buffer, size_t size); + + int RXsendPacket(int id, int Address); + int RXsendPacket(int id, int Address, int size); + + int flush(); + + class Packet { + bool freeData; + public: + unsigned char *data; + size_t data_size; + + // wrap a received data stream in an Packet object for analysis + Packet(unsigned char *data, size_t size); + // build a packet into the pre-allocated data array + // if data is null it will be malloc'ed and free'd on destruction. + + Packet( + unsigned char *data, + size_t size, + unsigned char id, + unsigned char instruction, + int parameter_data_size, + ...); + ~Packet(); + unsigned char getId(); + int getLength(); + int getSize(); + int getParameterCount(); + unsigned char getInstruction(); + unsigned char getParameter(int n); + bool isValid(); + + void toStream(Stream &stream); + + }; }; #endif diff --git a/firmware/dxl_pro.cpp b/dxl_pro.cpp similarity index 97% rename from firmware/dxl_pro.cpp rename to dxl_pro.cpp index bc499e3..8d17587 100644 --- a/firmware/dxl_pro.cpp +++ b/dxl_pro.cpp @@ -8,9 +8,9 @@ #include "dxl_pro.h" /* gbpRxBuffer gbDXLWritePointer -extern uint32 Dummy(uint32 tmp); -extern void uDelay(uint32 uTime); -extern void nDelay(uint32 nTime);*/ +extern uint32_t Dummy(uint32_t tmp); +extern void uDelay(uint32_t uTime); +extern void nDelay(uint32_t nTime);*/ unsigned short update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size) diff --git a/firmware/dxl_pro.h b/dxl_pro.h similarity index 97% rename from firmware/dxl_pro.h rename to dxl_pro.h index 88d1a87..584e304 100644 --- a/firmware/dxl_pro.h +++ b/dxl_pro.h @@ -12,7 +12,6 @@ extern "C" { #endif -#include "libpandora_types.h" #define MAXNUM_TXPACKET (255)//(65535) #define MAXNUM_RXPACKET (255)//(65535) diff --git a/examples/XL320_servo_example/XL320_servo_example.ino b/examples/XL320_servo_example/XL320_servo_example.ino new file mode 100644 index 0000000..f492c5d --- /dev/null +++ b/examples/XL320_servo_example/XL320_servo_example.ino @@ -0,0 +1,72 @@ + +// ======================================== +// Dynamixel XL-320 Arduino library example +// ======================================== + +// Read more: +// https://github.com/hackerspace-adelaide/XL320 + +#include "XL320.h" + +// Name your robot! +XL320 robot; + +// If you want to use Software Serial, uncomment this line +#include + +// Set the SoftwareSerial RX & TX pins +SoftwareSerial mySerial(10, 11); // (RX, TX) + +// Set some variables for incrementing position & LED colour +char rgb[] = "rgbypcwo"; +int servoPosition = 0; +int ledColour = 0; + +// Set the default servoID to talk to +int servoID = 1; + +void setup() { + + // Talking standard serial, so connect servo data line to Digital TX 1 + // Comment out this line to talk software serial + Serial.begin(115200); + + // Setup Software Serial + mySerial.begin(115200); + + // Initialise your robot + robot.begin(Serial); // Hand in the serial object you're using + + // I like fast moving servos, so set the joint speed to max! + robot.setJointSpeed(servoID, 1023); + +} + +void loop() { + + // LED test.. let's randomly set the colour (0-7) +// robot.LED(servoID, &rgb[random(0,7)] ); + + // LED test.. select a random servoID and colour + robot.LED(random(1,4), &rgb[random(0,7)] ); + + // LED colour test.. cycle between RGB, increment the colour and return 1 after 3 +// robot.LED(servoID, &rgb[ledColour]); + ledColour = (ledColour + 1) % 3; + + // Set a delay to account for the receive delay period + delay(100); + + // Servo test.. let's randomly set the position (0-1023) +// robot.moveJoint(servoID, random(0, 1023)); + + // Servo test.. select a random servoID and colour + robot.moveJoint(random(1,4), random(0, 1023)); + + // Servo test.. increment the servo position by 100 each loop +// robot.moveJoint(servoID, servoPosition); + servoPosition = (servoPosition + 100) % 1023; + + // Set a delay to account for the receive delay period + delay(100); +} \ No newline at end of file diff --git a/examples/XL320_servo_example_with_half_duplex_hardware_serial/XL320_servo_example_with_half_duplex_hardware_serial.ino b/examples/XL320_servo_example_with_half_duplex_hardware_serial/XL320_servo_example_with_half_duplex_hardware_serial.ino new file mode 100644 index 0000000..e52d6d3 --- /dev/null +++ b/examples/XL320_servo_example_with_half_duplex_hardware_serial/XL320_servo_example_with_half_duplex_hardware_serial.ino @@ -0,0 +1,86 @@ + +// ======================================== +// Dynamixel XL-320 Arduino library example +// ======================================== + +// Read more: +// https://github.com/hackerspace-adelaide/XL320 + +#include "XL320.h" + +// NOTE: WORK IN PROGRESS! +// For Half Duplex Hardware Serial to read data back from the servos +#include + +// Name your robot! +XL320 robot; + +// Set some variables for incrementing position & LED colour +char rgb[] = "rgbypcwo"; +int servoPosition = 0; +int ledColour = 0; + +// Delay between sending & receiving data +int delayBetweenSendReceive = 500; + +// Set the default servoID to talk to +int servoID = 3; + +void setup() { + + // You have to manually set the TX and RX lines of your chosen serial port to INPUT_PULLUP + // as this is the state they will be in when they are disconnected from the UART + // you also need to physically link the TX and RX pins in your circuit + // You can achieve this by connecting the data line out of the servo back into pin 0 RX + pinMode(0, INPUT_PULLUP); + pinMode(1, INPUT_PULLUP); + + pinMode(13, OUTPUT); + + // Talk serial with your servo by connecting the servo input data line to Digital TX 1 + HalfDuplexSerial.begin(115200); + HalfDuplexSerial.setTimeout(delayBetweenSendReceive); + + // Initialise your robot + robot.begin(HalfDuplexSerial); // Hand in the serial object you're using + + // I like fast moving servos, so set the joint speed to max! + robot.setJointSpeed(servoID, 1023); + delay(delayBetweenSendReceive); + +} + +void loop() { + + // LED test.. let's randomly set the colour (0-7) +// robot.LED(servoID, &rgb[random(0,7)] ); + + // LED test.. select a random servoID and colour + robot.LED(servoID, &rgb[ledColour] ); + + // LED colour test.. cycle between RGB, increment the colour and return 1 after 3 + ledColour = (ledColour + 1) % 3; + + // Set a delay to account for the receive delay period + delay(delayBetweenSendReceive); + + // Servo test.. select a random servoID and colour + robot.moveJoint(servoID, servoPosition); + + // Servo test.. increment the servo position by 100 each loop + servoPosition = (servoPosition + 100) % 1023; + + // Set a delay to account for the receive delay period + delay(delayBetweenSendReceive); + + // To read back from the servo + // TODO: Fix this.. getting invalid packets.. + byte buffer[256]; + XL320::Packet p = XL320::Packet(buffer, robot.readPacket(buffer,256)); + if (p.isValid()) { + // Set pin 13 HIGH so we know! + digitalWrite(13, HIGH); + } else { + digitalWrite(13, LOW); + } +} \ No newline at end of file diff --git a/examples/XL320_servo_set_baud_rate_or_id/XL320_servo_set_baud_rate_or_id.ino b/examples/XL320_servo_set_baud_rate_or_id/XL320_servo_set_baud_rate_or_id.ino new file mode 100644 index 0000000..64a8052 --- /dev/null +++ b/examples/XL320_servo_set_baud_rate_or_id/XL320_servo_set_baud_rate_or_id.ino @@ -0,0 +1,52 @@ + +// ======================================== +// Dynamixel XL-320 Arduino library example +// ======================================== + +// Read more: +// https://github.com/hackerspace-adelaide/XL320 + +#include "XL320.h" + +// Name your robot! +XL320 robot; + +void setup() { + + // Talking standard serial, so connect servo data line to Digital TX 1 + // Set the default servo baud rate which is 1000000 (1Mbps) if it's a brand new servo + Serial.begin(1000000); + + // Initialise your robot + robot.begin(Serial); // Hand in the serial object you're using + delay(100); + + // Current servoID + int servoID = 254; + + // NOTE: comment out either the XL_BAUD_RATE or XL_ID, only send one at a time + + // =================================== + // Set the serial connection baud rate + // =================================== + + // writePacket(1, XL_BAUD_RATE, x) sets the baud rate: + // 0: 9600, 1:57600, 2:115200, 3:1Mbps + robot.sendPacket(servoID, XL_BAUD_RATE, 2); + + // ================ + // Set the servo ID + // ================ + + // writePacket(1, XL_ID, x) sets the baud rate: + // ID can be between 1 and 253 (but not 200) +// robot.sendPacket(servoID, XL_ID, 3); +} + +void loop() { + // NOTE: load this sketch to the Arduino > Servo + // Then power cycle the Arduino + Servo + + // NOTE: When setting the servo ID, the baud rate defaults down to 9600 + // So then you'll need to power cycle, and re-write the baud rate to whatever you want via 9600 +} \ No newline at end of file diff --git a/firmware/HelloSpoon-Spark.cpp b/firmware/HelloSpoon-Spark.cpp deleted file mode 100644 index 0de9932..0000000 --- a/firmware/HelloSpoon-Spark.cpp +++ /dev/null @@ -1,507 +0,0 @@ -/* - - Code based on: - Dynamixel.cpp - Ax-12+ Half Duplex USART Comunication - Copyright (c) 2011 Savage Electronics. - And Dynamixel Pro library for OpenCM-9.04 made by ROBOTIS, LTD. - - Modified to work only with Dynamixel XL-320 actuator. - - Modifications made by Luis G III for HelloSpoon robot. - Webpage: http://hellospoonrobot.com - Twitter: @HelloSpoon - Youtube: http://youtube.com/user/hellospoonrobot - - This file can be used and be modified by anyone, - don't forget to say thank you to OP! - - */ -#ifdef SPARK -#include "application.h" //Spark Core compatible library -#else -#if defined(ARDUINO) && ARDUINO >= 100 // Arduino IDE Version -#include "Arduino.h" -#else -#include "WProgram.h" -#endif -#endif - -#include "dxl_pro.h" -#include "HelloSpoon-Spark.h" - -// Macro for the selection of the Serial Port -#define sendData(args) (Serial1.write(args)) // Write Over Serial -#define beginCom(args) (Serial1.begin(args)) // Begin Serial Comunication -#define readData() (Serial1.read()) - -// Select the Switch to TX/RX Mode Pin -#define setDPin(DirPin,Mode) (pinMode(DirPin,Mode)) -#define switchCom(DirPin,Mode) (digitalWrite(DirPin,Mode)) // Switch to TX/RX Mode - -#define NANO_TIME_DELAY 12000 - -int sendPacket(int ID, int Address, int value); -int RXsendPacket(int ID, int Address); -void nDelay(uint32 nTime); - -HelloSpoon::HelloSpoon() { - // TODO Auto-generated constructor stub - -} - -HelloSpoon::~HelloSpoon() { - // TODO Auto-generated destructor stub -} - -void HelloSpoon::begin() -{ - setDPin(Direction_Pin=4,OUTPUT); - beginCom(1000000); -} - -void HelloSpoon::moveJoint(int Joint, int value){ - int Address = XL_GOAL_POSITION_L; - - if(Joint == 1){ - sendPacket(1, Address, value); - nDelay(NANO_TIME_DELAY); - } - else if(Joint == 2){ - sendPacket(2, Address, value); - nDelay(NANO_TIME_DELAY); - sendPacket(3, Address, 1023-value); - nDelay(NANO_TIME_DELAY); - } - else{ - sendPacket(Joint+1, Address, value); - nDelay(NANO_TIME_DELAY); - } - - Serial1.flush(); -} - -void HelloSpoon::setJointSpeed(int Joint, int value){ - int Address = XL_GOAL_SPEED_L; - if(Joint == 1){ - sendPacket(1, Address, value); - nDelay(NANO_TIME_DELAY); - } - else if(Joint == 2){ - sendPacket(2, Address, value); - nDelay(NANO_TIME_DELAY); - sendPacket(3, Address, value); - nDelay(NANO_TIME_DELAY); - } - else{ - sendPacket(Joint+1, Address, value); - nDelay(NANO_TIME_DELAY); - } - Serial1.flush(); -} - -void HelloSpoon::LED(int Joint, char led_color[]){ - int Address = XL_LED; - int val = 0; - - if(led_color[0] == 'r'){ - val = 1; - } - - else if(led_color[0] == 'g'){ - val = 2; - } - - else if(led_color[0] == 'y'){ - val = 3; - } - - else if(led_color[0] == 'b'){ - val = 4; - } - - else if(led_color[0] == 'p'){ - val = 5; - } - - else if(led_color[0] == 'c'){ - val = 6; - } - - else if(led_color[0] == 'w'){ - val = 7; - } - - else if(led_color[0] == 'o'){ - val = 0; - } - - if(Joint == 1){ - sendPacket(1, Address, val); - nDelay(NANO_TIME_DELAY); - } - else if(Joint == 2){ - sendPacket(2, Address, val); - nDelay(NANO_TIME_DELAY); - sendPacket(3, Address, val); - nDelay(NANO_TIME_DELAY); - } - else{ - sendPacket(Joint+1, Address, val); - nDelay(NANO_TIME_DELAY); - } - Serial1.flush(); -} - -void HelloSpoon::setJointTorque(int Joint, int value){ - int Address = XL_GOAL_TORQUE; - if(Joint == 1){ - sendPacket(1, Address, value); - nDelay(NANO_TIME_DELAY); - } - else if(Joint == 2){ - sendPacket(2, Address, value); - nDelay(NANO_TIME_DELAY); - sendPacket(3, Address, value); - nDelay(NANO_TIME_DELAY); - } - else{ - sendPacket(Joint+1, Address, value); - nDelay(NANO_TIME_DELAY); - } - Serial1.flush(); -} - -void HelloSpoon::TorqueON(int Joint){ - - int Address = XL_TORQUE_ENABLE; - int value = 1; - - if(Joint == 1){ - sendPacket(1, Address, value); - nDelay(NANO_TIME_DELAY); - } - else if(Joint == 2){ - sendPacket(2, Address, value); - nDelay(NANO_TIME_DELAY); - sendPacket(3, Address, value); - nDelay(NANO_TIME_DELAY); - } - else{ - sendPacket(Joint+1, Address, value); - nDelay(NANO_TIME_DELAY); - } - Serial1.flush(); -} - -void HelloSpoon::TorqueOFF(int Joint){ - - int Address = XL_TORQUE_ENABLE; - int value = 0; - - if(Joint == 1){ - sendPacket(1, Address, value); - nDelay(NANO_TIME_DELAY); - } - else if(Joint == 2){ - sendPacket(2, Address, value); - nDelay(NANO_TIME_DELAY); - sendPacket(3, Address, value); - nDelay(NANO_TIME_DELAY); - } - else{ - sendPacket(Joint+1, Address, value); - nDelay(NANO_TIME_DELAY); - } - Serial1.flush(); -} - -void HelloSpoon::activateTrunk(){ - for(int id = 1; id < 6; id++){ - sendPacket(id, XL_TORQUE_ENABLE, 1); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - sendPacket(id, XL_LED, 4); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - } - -} - -void HelloSpoon::deactivateTrunk(){ - for(int id = 1; id < 6; id++){ - sendPacket(id, XL_TORQUE_ENABLE, 0); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - sendPacket(id, XL_LED, 5); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - } -} - -void HelloSpoon::quickTest(){ - - int position_tmp = 0; - - for(int id = 1; id < 6; id++){ - sendPacket(id, XL_LED, random(1,7)); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - sendPacket(id, XL_GOAL_SPEED_L, 200); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - } - - for(int id = 1; id < 6; id++){ - - position_tmp = random(0,512); - - if(id != 3){ - sendPacket(id, XL_GOAL_POSITION_L, position_tmp); - delay(1000); - Serial1.flush(); - } - - else{ - sendPacket(3, XL_GOAL_POSITION_L, 512-position_tmp); - delay(1000); - Serial1.flush(); - } - } - - for(int id = 1; id < 6; id++){ - sendPacket(id, XL_LED, 2); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - sendPacket(id, XL_GOAL_SPEED_L, 1023); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - } - - for(int id = 1; id < 6; id++){ - sendPacket(id, XL_LED, 0); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - } - -} - -int HelloSpoon::getSpoonLoad(){ - int spoon = RXsendPacket(5, XL_PRESENT_LOAD); - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - return spoon; -} - -int HelloSpoon::getJointPosition(int Joint){ - int pos = 0; - switch(Joint){ - case 1: pos = RXsendPacket(1, XL_PRESENT_POSITION); - break; - case 2: pos = RXsendPacket(2, XL_PRESENT_POSITION); - break; - case 3: pos = RXsendPacket(4, XL_PRESENT_POSITION); - break; - case 4: pos = RXsendPacket(5, XL_PRESENT_POSITION); - break; - } - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - return pos; -} - -int HelloSpoon::getJointSpeed(int Joint){ - int speed = 0; - switch(Joint){ - case 1: speed = RXsendPacket(1, XL_PRESENT_SPEED); - break; - case 2: speed = RXsendPacket(2, XL_PRESENT_SPEED); - break; - case 3: speed = RXsendPacket(4, XL_PRESENT_SPEED); - break; - case 4: speed = RXsendPacket(5, XL_PRESENT_SPEED); - break; - } - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - return speed; -} - -int HelloSpoon::getJointLoad(int Joint){ - int load = 0; - switch(Joint){ - case 1: load = RXsendPacket(1, XL_PRESENT_LOAD); - break; - case 2: load = RXsendPacket(2, XL_PRESENT_LOAD); - break; - case 3: load = RXsendPacket(4, XL_PRESENT_LOAD); - break; - case 4: load = RXsendPacket(5, XL_PRESENT_LOAD); - break; - } - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - return load; -} - -int HelloSpoon::getJointTemperature(int Joint){ - int temp = 0; - switch(Joint){ - case 1: temp = RXsendPacket(1, XL_PRESENT_TEMPERATURE); - break; - case 2: temp = RXsendPacket(2, XL_PRESENT_TEMPERATURE); - break; - case 3: temp = RXsendPacket(4, XL_PRESENT_TEMPERATURE); - break; - case 4: temp = RXsendPacket(5, XL_PRESENT_TEMPERATURE); - break; - } - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - return temp; -} - -int HelloSpoon::isJointMoving(int Joint){ - int motion = 0; - switch(Joint){ - case 1: motion = RXsendPacket(1, XL_MOVING); - break; - case 2: motion = RXsendPacket(2, XL_MOVING); - break; - case 3: motion = RXsendPacket(4, XL_MOVING); - break; - case 4: motion = RXsendPacket(5, XL_MOVING); - break; - } - nDelay(NANO_TIME_DELAY); - Serial1.flush(); - return motion; -} - -int sendPacket(int ID, int Address, int value){ - - /*Dynamixel 2.0 communication protocol - used by Dynamixel XL-320 and Dynamixel PRO only. - */ - - word cont, wchecksum, wpacklen; - volatile char gbpParamEx[130+10]; - unsigned char Direction_Pin; - - byte txbuffer[255]; - - gbpParamEx[0] = (unsigned char)DXL_LOBYTE(Address); - gbpParamEx[1] = (unsigned char)DXL_HIBYTE(Address); - - gbpParamEx[2] = DXL_LOBYTE(value); - gbpParamEx[3] = DXL_HIBYTE(value); - - txbuffer[0] = 0xff; - txbuffer[1] = 0xff; - txbuffer[2] = 0xfd; - txbuffer[3] = 0x00; - - txbuffer[4] = ID; - txbuffer[5] = DXL_LOBYTE(4+3); - txbuffer[6] = DXL_HIBYTE(4+3); - - txbuffer[7] = 0x03; - - for(cont = 0; cont < 4; cont++) - { - txbuffer[cont+8] = gbpParamEx[cont]; - } - - wchecksum = 0; - - wpacklen = DXL_MAKEWORD(txbuffer[5], txbuffer[6])+5; - if(wpacklen > (MAXNUM_TXPACKET)){ - return 0; - } - - wchecksum = update_crc(0, txbuffer, wpacklen); - txbuffer[wpacklen] = DXL_LOBYTE(wchecksum); - txbuffer[wpacklen+1] = DXL_HIBYTE(wchecksum); - - wpacklen += 2; - - switchCom(Direction_Pin, Tx_MODE); - - for(cont = 0; cont < wpacklen; cont++) - { - sendData(txbuffer[cont]); - nDelay(NANO_TIME_DELAY); - } - - //Clean buffer - for(cont = 0; cont < wpacklen; cont++) - { - txbuffer[cont] = 0; - } - //switchCom(Direction_Pin, Rx_MODE); - -} - -void nDelay(uint32 nTime){ - uint32 max; - for( max=0; max < nTime; max++){ - - } -} - -int RXsendPacket(int ID, int Address){ - - /*Dynamixel 2.0 communication protocol - used by Dynamixel XL-320 and Dynamixel PRO only. - */ - - word cont, wchecksum, wpacklen; - volatile char gbpParamEx[130+10]; - unsigned char Direction_Pin; - - byte txbuffer[255]; - - gbpParamEx[0] = (unsigned char)DXL_LOBYTE(Address); - gbpParamEx[1] = (unsigned char)DXL_HIBYTE(Address); - - gbpParamEx[2] = 2; - gbpParamEx[3] = 0; - - txbuffer[0] = 0xff; - txbuffer[1] = 0xff; - txbuffer[2] = 0xfd; - txbuffer[3] = 0x00; - - txbuffer[4] = ID; - txbuffer[5] = DXL_LOBYTE(4+3); - txbuffer[6] = DXL_HIBYTE(4+3); - - txbuffer[7] = 0x03; - - for(cont = 0; cont < 4; cont++) - { - txbuffer[cont+8] = gbpParamEx[cont]; - } - - wchecksum = 0; - - wpacklen = DXL_MAKEWORD(txbuffer[5], txbuffer[6])+5; - if(wpacklen > (MAXNUM_TXPACKET)){ - return 0; - } - - wchecksum = update_crc(0, txbuffer, wpacklen); - txbuffer[wpacklen] = DXL_LOBYTE(wchecksum); - txbuffer[wpacklen+1] = DXL_HIBYTE(wchecksum); - - wpacklen += 2; - - switchCom(Direction_Pin, Tx_MODE); - - for(cont = 0; cont < wpacklen; cont++) - { - sendData(txbuffer[cont]); - } - - switchCom(Direction_Pin, Rx_MODE); - -} diff --git a/firmware/factoryReset/HelloSpoon_FactoryCode.ino b/firmware/factoryReset/HelloSpoon_FactoryCode.ino deleted file mode 100644 index c754452..0000000 --- a/firmware/factoryReset/HelloSpoon_FactoryCode.ino +++ /dev/null @@ -1,82 +0,0 @@ -/* -This is the original code provided with your HelloSpoon. - -All the Mobile Suite from HelloSpoon will work -while this code is loaded in your robot. -If you load a new code directly from Spark IDE (http://spark.io/build) -all the Mobile Suite (Animator, Controller and HelloSpoon App) will stop working. - -To learn how to upload this code and custom codes to HelloSpoon -please visit the Wiki included in this repository. - -Hope this helps you! -Happy coding :-)! - -- QOLbotics Team. - -*/ - -#include "HelloSpoon-Spark/HelloSpoon-Spark.h" -#include "Serial2/Serial2.h" - -SYSTEM_MODE(SEMI_AUTOMATIC); //This runs setup() immidiately and you have to manually connect to the Spark Cloud. - -HelloSpoon robot; - -void setup() { - - //EEPROM.write(1, 0); //Mandatory use the first time you load the code, then please comment it. - - if(EEPROM.read(1) == 0){ - HelloSpoon_FirstUsage(); - } - else{ - HelloSpoon_setup(); - } - -} - -void loop() { - - if(Serial2.available()){ - if(Serial2.read()=='w'){ - HelloSpoon_Online(); - } - } - -} - -void HelloSpoon_setup(){ - /*Start HelloSpoon robot*/ - robot.begin(); - /*Start Bluetooth communication*/ - Serial2.begin(57600); - /*Turn off Wifi module to save battery*/ - WiFi.off(); - delay(1000); - /*Activate HelloSpoon's trunk*/ - robot.activateTrunk(); -} - -void HelloSpoon_Online(){ - /*Turn on WiFi module*/ - WiFi.on(); - delay(1000); - /*Connect to the already known Wifi network*/ - WiFi.connect(); - delay(1000); - /*Connect to Spark Cloud*/ - Spark.connect(); -} - -void HelloSpoon_FirstUsage(){ - EEPROM.write(1, 0x01); //Change value on EEPROM's second byte. - /*Turn on WiFi module*/ - WiFi.on(); - delay(1000); - /*Connect to the already known Wifi network*/ - WiFi.connect(); - delay(1000); - /*Connect to Spark Cloud*/ - Spark.connect(); -} diff --git a/firmware/libpandora_types.h b/firmware/libpandora_types.h deleted file mode 100644 index cf5ae42..0000000 --- a/firmware/libpandora_types.h +++ /dev/null @@ -1,79 +0,0 @@ -/****************************************************************************** - * The MIT License - * - * Copyright (c) 2010 Perry Hung. - * - * Permission is hereby granted, free of charge, to any person - * obtaining a copy of this software and associated documentation - * files (the "Software"), to deal in the Software without - * restriction, including without limitation the rights to use, copy, - * modify, merge, publish, distribute, sublicense, and/or sell copies - * of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be - * included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, - * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS - * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN - * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - *****************************************************************************/ - -/** - * @file libpandora_types.h - * [ROBOTIS] change file name from libmaple_types.h to libpandora_types.h - * @brief libmaple types - */ - -#ifndef _LIBMAPLE_TYPES_H_ -#define _LIBMAPLE_TYPES_H_ - -#define NVIC_BASE ((struct nvic_reg_map*)0xE000E100) - -typedef unsigned char uint8; -typedef unsigned short uint16; -typedef unsigned int uint32; -typedef unsigned long long uint64; - -typedef signed char int8; -typedef short int16; -typedef int int32; -typedef long long int64; - -/** - * [ROBOTIS][START] support for Dynamixel SDK. - * */ -#define word uint16 -#define byte uint8 -/** - * [ROBOTIS][END] support for Dynamixel SDK. - * */ - -/* - * [ROBOTIS][START]2012-12-13 to support user interrupt func. - * */ - -typedef void (*voidFuncPtrUart)(byte); -typedef void (*voidFuncPtrUsb)(byte*, byte ); - -/* - * [ROBOTIS][END]2012-12-13 to support user interrupt func. - * */ - -typedef void (*voidFuncPtr)(void); - -#define __io volatile -#define __attr_flash __attribute__((section (".USER_FLASH"))) - -#ifndef NULL -#define NULL 0 -#endif - - - -#endif diff --git a/spark.json b/spark.json deleted file mode 100644 index e9fa5f5..0000000 --- a/spark.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "name": "HelloSpoon-Spark", - "author": "HelloSpoon Embedded Team ", - "license": "GNU", - "version": "0.0.4", - "description": "A library to control HelloSpoon robot." -}