RoboSavvy Forum Robosavvy Forum: The largest online community of Humanoid Robot Builders 2006-12-11T09:08:04+01:00 http://forum.robosavvy.com/feed.php?f=1&t=765 2006-12-11T09:08:04+01:00 2006-12-11T09:08:04+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=5152#p5152 <![CDATA[Programmable robot]]> Are there any library source for C++?

Bullit wrote:
Here are some links for avr programming.
Procyon AVRlib
ARVfreaks
Peter turner's very useful web site and links
I have not programmed the bioloid in C but I have programmed the CM-2 for the cycloid II in C and its very easy. The tools are all free and there are many libraries available just not one specific for the bioloid. If you check the tribotix site you will find an example.c program that includes some basic communications functions for communicating with the dynamixels. At Tribotix, look under CM-2. I'm sure this is compatable with the CM-5 as they are almost identical.
I hope this helps.

Statistics: Posted by ryann2k1 — Mon Dec 11, 2006 9:08 am

]]> 2006-11-12T15:45:47+01:00 2006-11-12T15:45:47+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4684#p4684 <![CDATA[Programmable robot]]>
the R/Txpacket routines are only superficially different.
I just tried to rewrite and comment B.S.Kim's (Robotis) code but dared not modify the protocol or the register sequencing.

I created a new sourceforge project: robosavvy.sourceforge.net
and the plan is to upload and maintain various code excerpts posted regularly on this forum using sourceforge's CVS or maybe better SVN(?).
The licensing on this sourceforge project is LGPL which means open-source code that may be freely used in commercial projects (unlike GPL).

If you or anyone else have a sourceforge user name, i'll gladly add you to the project group so you can maintain your source code revisions ro compiled applications through their great system and share it with everyone.

Statistics: Posted by limor — Sun Nov 12, 2006 3:45 pm

]]> 2006-11-12T14:54:15+01:00 2006-11-12T14:54:15+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4683#p4683 <![CDATA[Programmable robot]]>
Code:
void setLED(byte servoID,byte onOff01)
{
  byte bTxPacketLength,bRxPacketLength;
 
  if (onOff01 > 1) onOff01 = 1;
  gbpParameter[0] = P_LED; //Address of LED
  gbpParameter[1] = onOff01;// 0 = Off, 1 = On
  bTxPacketLength = TxPacket(servoID,INST_WRITE,2);
}

void setTorque(byte servoID,uint16_t torque)
{
  byte th,tl,bTxPacketLength,bRxPacketLength;

  th = torque >> 8;
  tl = torque & 0xff;

  gbpParameter[0] = P_TORQUE_LIMIT_L;
  gbpParameter[1] = tl;
  gbpParameter[2] = th;
  bTxPacketLength = TxPacket(servoID,INST_WRITE,3);
   // now enable toque limit
  TorqueEnable(servoID,1);
  return;
}

void TorqueEnable(byte servoID,byte onoff01)
{
  byte bTxPacketLength,bRxPacketLength;

  gbpParameter[0] = P_TORQUE_ENABLE;
  gbpParameter[1] = onoff01; // OnOff
  bTxPacketLength = TxPacket(servoID,INST_WRITE,2);
  return;
}

void setSpeed(byte servoID,uint16_t speed)
{
  byte th,tl,bTxPacketLength,bRxPacketLength;

  th = speed >> 8;
  tl = speed & 0xff;

  gbpParameter[0] = P_GOAL_SPEED_L;
  gbpParameter[1] = tl;
  gbpParameter[2] = th;
  bTxPacketLength = TxPacket(servoID,INST_WRITE,3);
  return;
}

void MoveTo(byte servoID,uint16_t pos,uint16_t speed,byte wait)
{

  byte ph,pl,sh,sl,bTxPacketLength,bRxPacketLength;
  uint16_t curpos;
  uint16_t count;
 
  ph = pos >> 8;
  pl = pos & 0xff;
  sh = speed >> 8;
  sl = speed & 0xff;
 
  gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
  gbpParameter[1] = pl; //Writing Data P_GOAL_POSITION_L
  gbpParameter[2] = ph; //Writing Data P_GOAL_POSITION_H
  gbpParameter[3] = sl; //Writing Data P_GOAL_SPEED_L
  gbpParameter[4] = sh; //Writing Data P_GOAL_SPEED_H
  bTxPacketLength = TxPacket(servoID,INST_WRITE,5);

  if(wait == 0) return;
  count = 0;
  while(1)
  {
   curpos = GetMotorPosition(servoID);
  if(curpos != 0xffff)
    {
    count++;
   if(curpos == pos || curpos == pos -1 || curpos == pos + 1)
     {
     TxDString("\r\n Move complete at = ");TxD32Dec((long)curpos);
     TxDString("\r\n measurements = ");TxD32Dec((long)count);
     return;
     }
   }
  }
}

uint16_t GetMotorPosition(byte servoID)
{
  byte bTxPacketLength,bRxPacketLength;
  uint16_t curpos;

  gbpParameter[0] = P_PRESENT_POSITION_L;
  gbpParameter[1] = 2; //Read Length
  bTxPacketLength = TxPacket(servoID,INST_READ,2);
  bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);

  if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
   {
    curpos = (gbpRxBuffer[6]<<8) + gbpRxBuffer[5];
   return curpos;
   }
  return 0xffff;
}

uint16_t GetMotorLoad(byte servoID)
{
  byte bTxPacketLength,bRxPacketLength;
  uint16_t curVal;

  gbpParameter[0] = P_PRESENT_LOAD_L;
  gbpParameter[1] = 2; //Read Length
  bTxPacketLength = TxPacket(servoID,INST_READ,2);
  bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);

  if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
   {
    curVal = (gbpRxBuffer[6]<<8) + gbpRxBuffer[5];
   return curVal;
   }
  return 0xffff;
}

uint16_t GetMotorTorque(byte servoID)
{
  byte bTxPacketLength,bRxPacketLength;
  uint16_t curVal;

  gbpParameter[0] = P_TORQUE_LIMIT_L;
  gbpParameter[1] = 2; //Read Length
  bTxPacketLength = TxPacket(servoID,INST_READ,2);
  bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
 
  if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
   {
    curVal = (gbpRxBuffer[6]<<8) + gbpRxBuffer[5];
   return curVal;
   }
  return 0xffff;
}

/*
TxPacket() send data to RS485.
TxPacket() needs 3 parameter; ID of Dynamixel, Instruction byte, Length of parameters.
TxPacket() return length of Return packet from Dynamixel.
*/
byte TxPacket(byte bID, byte bInstruction, byte bParameterLength)
{
    byte bCount,bCheckSum,bPacketLength;

    gbpTxBuffer[0] = 0xff;
    gbpTxBuffer[1] = 0xff;
    gbpTxBuffer[2] = bID;
    gbpTxBuffer[3] = bParameterLength+2; //Length(Paramter,Instruction,Checksum)
    gbpTxBuffer[4] = bInstruction;
    for(bCount = 0; bCount < bParameterLength; bCount++)
    {
        gbpTxBuffer[bCount+5] = gbpParameter[bCount];
    }
    bCheckSum = 0;
    bPacketLength = bParameterLength+4+2;
    for(bCount = 2; bCount < bPacketLength-1; bCount++) //except 0xff,checksum
    {
        bCheckSum += gbpTxBuffer[bCount];
    }
    gbpTxBuffer[bCount] = ~bCheckSum; //Writing Checksum with Bit Inversion

    RS485_TXD;
    for(bCount = 0; bCount < bPacketLength; bCount++)
    {
        sbi(UCSR0A,6);//SET_TXD0_FINISH;
        TxD80(gbpTxBuffer[bCount]);
    }
    while(!CHECK_TXD0_FINISH); //Wait until TXD Shift register empty
    RS485_RXD;
    return(bPacketLength);
}

/*
RxPacket() read data from buffer.
RxPacket() need a Parameter; Total length of Return Packet.
RxPacket() return Length of Return Packet.
*/

byte RxPacket(byte bRxPacketLength)
{
#define RX_TIMEOUT_COUNT2   3000L 
#define RX_TIMEOUT_COUNT1  (RX_TIMEOUT_COUNT2*10L) 
  unsigned long ulCounter;
  byte bCount, bLength, bChecksum;
  byte bTimeout;

  bTimeout = 0;
  for(bCount = 0; bCount < bRxPacketLength; bCount++)
  {
    ulCounter = 0;
    while(gbRxBufferReadPointer == gbRxBufferWritePointer)
    {
      if(ulCounter++ > RX_TIMEOUT_COUNT1)
      {
        bTimeout = 1;
        break;
      }
    }
    if(bTimeout) break;
    gbpRxBuffer[bCount] = gbpRxInterruptBuffer[gbRxBufferReadPointer++];
  }
  bLength = bCount;
  bChecksum = 0;
 
  if(gbpTxBuffer[2] != BROADCASTING_ID)
  {
    if(bTimeout && bRxPacketLength != 255)
    {
      if(bVerbose == 1)
       {
      TxDString("\r\n [Error:RxD Timeout]");
        }     
     CLEAR_BUFFER;
    }
   
    if(bLength > 3) //checking is available.
    {
      if(gbpRxBuffer[0] != 0xff || gbpRxBuffer[1] != 0xff )
      {
        if(bVerbose == 1)
         {
         TxDString("\r\n [Error:Wrong Header]");
         }
      CLEAR_BUFFER;
        return 0;
      }
      if(gbpRxBuffer[2] != gbpTxBuffer[2] )
      {
        if(bVerbose == 1)
         {
         TxDString("\r\n [Error:TxID != RxID]");
         }
      CLEAR_BUFFER;
        return 0;
      } 
      if(gbpRxBuffer[3] != bLength-4)
      {
        if(bVerbose == 1)
         {
         TxDString("\r\n [Error:Wrong Length]");
         }
        CLEAR_BUFFER;
        return 0;
      } 
      for(bCount = 2; bCount < bLength; bCount++) bChecksum += gbpRxBuffer[bCount];
      if(bChecksum != 0xff)
      {
        if(bVerbose == 1)
         {
         TxDString("\r\n [Error:Wrong CheckSum]");
         }
        CLEAR_BUFFER;
        return 0;
      }
    }
  }
  return bLength;
}



again, I use these for the CM-2 but they should be identical.

Statistics: Posted by Bullit — Sun Nov 12, 2006 2:54 pm

]]> 2006-11-12T13:59:50+01:00 2006-11-12T13:59:50+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4682#p4682 <![CDATA[Programmable robot]]>
I've uploaded some code for the Bioloid.
It is a rewrite of the example.c that comes with the kit.

http://robosavvy.com/Builders/limor/Bioloid_example.zip

The pnproj file is a Programmers Notepad Project file. WinAVR (gcc for Atmel) comes with PN as the default IDE. It uses gnu make to compile the project. The makefile only needs a couple of lines modified to fit your project files.

This code is not yet fully functional but it demonstrates how Atmega128 is programmed and how the Robotis servo protocol works.

It should be compatible eventually with both the CM5 and with the Bioloid sensor board by Pepperm (search the Bioloid forum).


For example, here is a routines that initialize the RS485 serial interface

Code:

/*
 * InitSerialRS485 - set the i/o pins to talk to servos
 * Interrupt - if 1 then input will be interrupt driven
 *             gInterruptBuffer[gInterruptBufferPointer++] will continously take new read values
 * Baudrate - used in conjunction with prescaler...
 */
void InitSerialRS485(byte Baudrate, byte Interrupt)
{

    UBRR0H = 0;
   UBRR0L = Baudrate;
    UCSR0A = 0x02; // U2X: Double the USART transmission speed
   UCSR0B = 0x18; // Receiver and Transmitter enable
    if(Interrupt) sbi(UCSR0B,7); // RxD interrupt enable
    UCSR0C = 0x06; // 8 bit data, no parity
   UDR0 = 0xFF; // this will be broadcast (may be required by servos?)
    SET_TXRS485_FINISH; // Note. set 1, then 0 is read
}



As you may notice, it is indeed C but 99% of the logic of what this code does is in the Atmega128 documentation. There is no object-oriented design patterns. This is back to basics, like writing a device driver. No operating system no JIT or garbage-collection or simulated threads or any other hidden agenda.

Here's another example, this time it is the actual implementation of the Robotis protocol

Code:

volatile byte Buffer[256]; // in interrupt UART read mode, gBuffer[gBufferPointer++] will continously take new read values
volatile byte BufferPointer=0;


/*
 * SIGNAL()  Rx Interrupt - write data to buffer (used with CM5's RS485)
 * When a byte (8bit + parity + stop) has been read by the Atmega, it issues an interrupt
 * RxBufferPointer goes back to 0 after 255, RxPacket doesn't need it to reset it to 0
 *
 * The reason for using an interrupt driven Rx is because the CM5 has 2 serials.
 * you can't poll two Rx because the other one may need attention during poll.
 */
SIGNAL (SIG_UART0_RECV)
{
   Buffer[BufferPointer++] = RXRS485_DATA;
}




/*
 * TxRS485() write byte to Robotis bus
 */
static void TxRS485(byte Data)
{
   while(!CHECK_TXRS485_READY);
   TXRS485_DATA = Data;
}
/*
 * RxRS485() read byte from Robotis bus (CM5 is normally interrupt Rx so doesnt use this routine)
 */
static byte RxRS485(void)
{
   while(!CHECK_RXRS485_READY);
   return(RXRS485_DATA);
}





/*
TxRS485Packet() send Robotis package to RS485.
Similar format packet is sent by master and by slave (5th byte differs)
ID - My ID
Instruction - Master: PING, READ etc. Slave: Error bitmask
ParameterLength - number of parameters sent in Buffer
Parameter - will be coppied to Buffer[5]..Buffer[5+ParameterLength-1]
*/
void TxRS485Packet(byte ID, byte Instruction, byte ParameterLength, byte *Parameter)
{
    byte i, CheckSum, PacketLength;

    Buffer[0] = 0xff;
    Buffer[1] = 0xff;
    Buffer[2] = ID;
    Buffer[3] = ParameterLength+2; //Length(Paramter,Instruction,Checksum)
    Buffer[4] = Instruction;
   for (i=0; i< ParameterLength;i++)
      Buffer[5+i] = Parameter[i];

    CheckSum = 0;
    PacketLength = ParameterLength+4+2;
    for(i = 2; i < PacketLength-1; i++) //except 0xff,checksum
    {
        CheckSum += Buffer[i];
    }
    Buffer[i] = ~CheckSum; //Writing Checksum with Bit Inversion

    SET_RS485_TXD;
    for(i = 0; i < PacketLength; i++)
    {
        SET_TXRS485_FINISH; // ? this only clears bit6. triggers interrupt if interrupt flag is set
        TxRS485(Buffer[i]);
    }
    while(!CHECK_TXRS485_FINISH); // Wait until (bit6) TXD Shift register empty
    SET_RS485_RXD;
   
   BufferPointer=0; // Response packet instantanous. Interrupt Rx relies on buffer being filled from 0
}

/*
 * RxRS485Packet() receive Robotis instruction packet through RS485.
 * This is is the non-interrupt version of the read-packet designed for a sensor board.
 * The full protocol implementation of the protocol is needed only by Robotis servos
 * returns the packet pointer if the packet is intended for me otherwise null
*/
byte *RxRS485Packet(byte ID)
{
    byte i;
   
   
    Buffer[0] = RxRS485(); // 0xff;
    Buffer[1] = RxRS485(); // 0xff;
    Buffer[2] = RxRS485(); // ID;
    Buffer[3] = RxRS485(); // ParameterLength+2; // Length(Paramter,Instruction,Checksum)
    Buffer[4] = RxRS485(); // Instruction;
   for (i=0; i<Buffer[3]-2; i++)
      Buffer[5+i] = RxRS485(); // Parameters..
   Buffer[5+i] = RxRS485(); // Checksum;
   
   // not performing any error checking. assumning that robotis cables are ok.
   
   return (Buffer[2] == ID)? Buffer: (byte *)0;

}

/*
 * RxRS485PacketIntr() receive Robotis (response) packet through RS485.
 * This is is the interrupt-driven version of the read-packet designed for CM5
 * Just waits for the buffer to fill up by the interrupt routine
 *
 * The reason for using an interrupt driven Rx is because the CM5 has 2 serials.
 * you can't poll two Rx because the other one may need attention during poll.
*/

byte *RxRS485PacketIntr()
{
   byte i;
   long r=0;
   
   while (BufferPointer < 4) if (r++ > 100000) return 0;
   while (BufferPointer < 4+Buffer[3]) if (r++ > 200000) return 0;

   return Buffer;

}




Finally here is the actual main() program. Hopefully when this library will be complete, you wont have to care about the details of the above implementation of the protocol and initialization, and focus only on getting servo position and other sensor data packets and sending back servo position and torque packets and you AI algorithms.

Code:

#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>

#include "robotis.h"



int main (void)
{
   unsigned short popo=300;
   byte i;
   byte *packet;
   byte buf[256];
   
   InitPorts();

   InitSerialRS485(BAUD_1000000, 1); // Rx is interrupt driven
   InitSerialPC(BAUD_57600, 0); // Rx is not interrupt driven

   sei(); // enable interrupts
   
   while (1) {
   
      TxPCString("Press Enter ->");
      RxPC(); // wait for PC keypress

   popo += 30;
   buf[0] = P_GOAL_POSITION_L;
   buf[1] = 0; // Data P_GOAL_POSITION_L
   buf[2] = 2; // Data P_GOAL_POSITION_H
   buf[3] = 0x00; // Data P_GOAL_SPEED_L
   buf[4] = 0x01; // Data P_GOAL_SPEED_H
   TxRS485Packet(6,INST_WRITE,5, buf);
   if (packet = RxRS485PacketIntr())   TxPCString("Bingo!\r\n");
   
   
      for (i=0; i <20; i++) {
         TxPCString("Testing [");
         TxPC32Dec(i);
         TxPCString("] ");
         TxRS485Packet(i, INST_PING, 0, 0);
         if (packet = RxRS485PacketIntr()) {
            TxPCString("Bingo!");
         }
         TxPCString("\r\n");
      }
   }
}

Statistics: Posted by limor — Sun Nov 12, 2006 1:59 pm

]]> 2006-11-12T13:45:51+01:00 2006-11-12T13:45:51+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4681#p4681 <![CDATA[Programmable robot]]> Procyon AVRlib
ARVfreaks
Peter turner's very useful web site and links
I have not programmed the bioloid in C but I have programmed the CM-2 for the cycloid II in C and its very easy. The tools are all free and there are many libraries available just not one specific for the bioloid. If you check the tribotix site you will find an example.c program that includes some basic communications functions for communicating with the dynamixels. At Tribotix, look under CM-2. I'm sure this is compatable with the CM-5 as they are almost identical.
I hope this helps.

Statistics: Posted by Bullit — Sun Nov 12, 2006 1:45 pm

]]> 2006-11-12T13:26:31+01:00 2006-11-12T13:26:31+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4680#p4680 <![CDATA[Programmable robot]]>
Reading and writing from/to servos is easy with your own program. The only thing you will miss is that you lose all standard CM5 abilities when you program your CM5 in C. Note that CM5 has an internal set of software registers, already made functions, and a BCP interpreter. You lose all that when you put your own C program into CM5. The reason is you replace the original program with your own. A small part remains which allows you to upload a new program or restore the original Bioloid code.

Other than that you can program the Bioloid in the language of your choice in your PC using remote control, that is, using the serial interface from your PC to send commands and receive data. This interface is used for example by Robot Terminal, a program that comes with Bioloid kit.

You can even prepare a BCP program that allows remote control using Zigbee wireless from your PC (or even Bluetooth if you perform the hack provided by Pev).

Statistics: Posted by inaki — Sun Nov 12, 2006 1:26 pm

]]> 2006-11-12T09:57:16+01:00 2006-11-12T09:57:16+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4679#p4679 <![CDATA[Programmable robot]]>
but do you know any tutorials which can help me to program the Bioloid in C?

actually I know how to program in C language in general but I don't know how programming a robot's microcontroller. There is certainly some C libraries which permit to program it. Isn't it?

thank you.

Statistics: Posted by freeforall — Sun Nov 12, 2006 9:57 am

]]> 2006-11-12T02:29:51+01:00 2006-11-12T02:29:51+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4674#p4674 <![CDATA[Programmable robot]]>
Yes, I believe the Bioloid is the best robot (that I have seen) as far as programming goes. It also happens to have the best servos (at least for a reasonable cost) and a great design. Robotis did nice work putting it together. But that is just opinion.

I think that if I wouldnt have gotten an RN then it would have been a Bioloid of me.

Gotta love robots :)

-Hive

Statistics: Posted by hivemind — Sun Nov 12, 2006 2:29 am

]]> 2006-11-12T02:16:38+01:00 2006-11-12T02:16:38+01:00 http://forum.robosavvy.com/viewtopic.php?t=765&p=4673#p4673 <![CDATA[Programmable robot]]>
Could you help me to choose a robot which permits programming in java or in C language ( I prefer java).

I did some research on the internet: the best robot I found is the bioloid. what do you think of this robot as far as programming in C is concerned? if you have any other suggestions I will be pleased to know them.

Moreover do you know any tutorials about programming the bioloid in C ?

Thank you for your help.

Statistics: Posted by freeforall — Sun Nov 12, 2006 2:16 am

]]>