(I) Robotis CM-900 C++ crash examples tutorial

This is the first example in a serie of cheap (absolutely free and cheap) articles to get some basic and practical C++ knowledge. It will references the great explanations provided at www.cplusplus.com C++ tutorial. And using the very cheap and opensource (about 20$/€) Robotis 32 bits microcontroller board CM-900 (STM32 based) and Arduino based IDE.

Here you can download the file with the code, and you can find links to a lot of free tutorials, courses and books to learn C++ here.


[sourcecode language="cpp"]
// C++ crash tutorial with Robotis CM-900.

// First example: http://softwaresouls.com/softwaresouls/2013/06/23/c-crash-tutorial-using-robotis-cm-900-board-and-ide-i/

/*
Hello World shows messages on construction and destruction
Also it lets you to salute.
Showing always its assigned identifiction number MyId
*/
class HelloWorld
{
private:
int myId; //Object identification num ber

public:

// class constructor http://www.cplusplus.com/doc/tutorial/classes/
HelloWorld(char *message, byte id)
{
myId=id;
SerialUSB.print(message);
printId();
}

// class destructor http://www.cplusplus.com/doc/tutorial/classes/
~HelloWorld()
{
SerialUSB.print ("Destructing object: ");
printId();
}

void printId()
{
SerialUSB.print(" ID:");
SerialUSB.println(myId);
SerialUSB.println(" ");
}

void Salute(char *name)
{
SerialUSB.print("Hi!, ");
SerialUSB.println(name);
SerialUSB.print("Regards from object: ");
printId();
}
};

/*
void setup() function it's only executed one time at the start of the execution.
It is called from a hidden main() function in the Ronbotis CM-900 IDE core.

\ROBOTIS-v0.9.9\hardware\robotis\cores\robotis\main.cpp
Note: "while (1)" is a forevr loop ( http://www.cplusplus.com/doc/tutorial/control ):

(Basic structure http://www.cplusplus.com/doc/tutorial/program_structure/)

int main(void) {
setup();

while (1) {
loop();
}
return 0;
}
*/
void setup()
{
SerialUSB.begin(); //initialize serial USB connection
delay(3000); //We will wait 3 seconds, let the user open (Control+Shift+M) the Monitor serial console
}

//We will not see neither the construction nor the destruction of this global object because serial port it's not still initiated
HelloWorld hw0("construction object", 0); //Object construction http://www.cplusplus.com/doc/tutorial/classes/

// A counter to see progress and launch events
int iterationCounter=0; //An integer variable to count iterations http://www.cplusplus.com/doc/tutorial/variables

// void loop() will be executing the sentences it contains while CM-900 is on.
void loop()
{
// Lets's show only the first 5 iterations http://www.cplusplus.com/doc/tutorial/control/
if (iterationCounter<5)
{
SerialUSB.print("starting iteration #");
SerialUSB.println(++iterationCounter); // firts, iterationCounter is incremented and then printed

// We will see the consttruction and destruction messages from this local (inside the loop function) object. Object construction http://www.cplusplus.com/doc/tutorial/classes/
HelloWorld hw1("constructing object", 1);
hw1.Salute("Joe");

if (iterationCounter==3)
{
// We will see the consttructiona and destruction messages from this local (inside the "if" block inside the "loop" function) object. Objet construction http://www.cplusplus.com/doc/tutorial/classes/
HelloWorld hw2("constructing object", 2);
hw2.Salute("Jones");
} // Objet hw2 destruction http://www.cplusplus.com/doc/tutorial/classes/

//Let's show that object hw0 is alive
hw0.Salute("Pepe");

SerialUSB.print("ending iteration #");
SerialUSB.println(iterationCounter++); // first cpunter is printed, then incremented.
} // Objet hw1 destruction http://www.cplusplus.com/doc/tutorial/classes/
} // Program end. Objet hw0 destruction http://www.cplusplus.com/doc/tutorial/classes/
[/sourcecode]

Robotis CM-900 in Toss Mode and querying connected sensors

CM-900 is a very cheap and tiny Robotis Dynamixel controller based in the STM32 ARM 32 bits microcontroller. Here you can find several links to documents.



I think it's ideal as a controller managed with, for example, a Raspberry Pi or a Pandaboard, so I have created a program which can be managed externally to control Dynamixel items (AX-12, AX-S1), Toss Mode, and sensors connected to the CM-900 with a simple protocol similar but simpler than the Dynamixel protocol:

[Headers] [ID] [Command code] [Nº Params] [Param1] [Param2] [ParamN]

Toss Mode works like USB2Dynamixel, but with this program in addition you can get sensor vales.

Toss Mode was a software feature, at least since CM-5, which allows you to use the elements connected to the controller from another device, usually a more powerful computer. What it receives from the USB or serial is sent to the Dynamixel bus, and whatever it receives from Dynamyxel bus is sent to the serial or USB connection.

Example for blinking twice: { 0xFF, 0xFF, 90, 1, 1, 2 }

Headers: 0xFF 0xFF

ID: 90

Command code: 1

Number of parameters: 1

Parameter 1: 2

Example for reading ultrasonic sensor (HC-SR04) : { 0xFF, 0xFF, 90, 2, 0 }

[sourcecode language="CPP"]

void MainWindow::testCM900_Sensor_US()
{
const int size=5;
byte buffer2[size] = { 0xFF, 0xFF, 90, 2, 0 };
pDynComm->rawWrite(buffer2, size);
}
[/sourcecode]

[caption id="attachment_1753" align="alignleft" width="320"] Ultrasonic sensor hc-sr04[/caption]

And here the file with the code for Robotis CM-9 IDE version 0.9.9. You also need to copy Prof. Mason library for ultrasonic sensor to CM 900 IDE libraries folder. Althought it i still a beta version, I think it works ok. Asigning true to these variables, the messages for debugging are sent either to USB connection o Serial2 (connecting Zigbee is the easy option):

[sourcecode language="CPP"]
bool debugOutputSerial2On=true;
bool debugOutputOn=false;
[/sourcecode]

Arduino LCD 1602 (16x2) display

Another little pearl from www.dx.com is the Arduino compatible LCD 1602 (16 characters each of the 2 rows) display:



It's really cheap, 6$/4.5€, works very fine and it's easy to use! I will use as a handy debug display and little dashboard (it has 6 buttons at the bottom) while on field robots debugging, but this wioll be a near post with the sourcecode (that also will be a order receiver to program a Rapsberry Pi to control a robot using and Arduino Funduino Duemilanove.)



Two easy examples from the Arduino IDE wondeful examples, it's important that you notice that the initialization sentence should be:

LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

The hello world
[sourcecode language="C"]
/*
LiquidCrystal Library - Hello World

Demonstrates the use a 16x2 LCD display. The LiquidCrystal
library works with all LCD displays that are compatible with the
Hitachi HD44780 driver. There are many of them out there, and you
can usually tell them by the 16-pin interface.

This sketch prints "Hello World!" to the LCD
and shows the time.

The circuit:
* LCD RS pin to digital pin 12
* LCD Enable pin to digital pin 11
* LCD D4 pin to digital pin 5
* LCD D5 pin to digital pin 4
* LCD D6 pin to digital pin 3
* LCD D7 pin to digital pin 2
* LCD R/W pin to ground
* 10K resistor:
* ends to +5V and ground
* wiper to LCD VO pin (pin 3)

Library originally added 18 Apr 2008
by David A. Mellis
library modified 5 Jul 2009
by Limor Fried (http://www.ladyada.net)
example added 9 Jul 2009
by Tom Igoe
modified 22 Nov 2010
by Tom Igoe

This example code is in the public domain.

http://www.arduino.cc/en/Tutorial/LiquidCrystal
*/

// include the library code:
#include <LiquidCrystal.h>

// initialize the library with the numbers of the interface pins
//LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

void setup() {
// set up the LCD's number of columns and rows:
lcd.begin(16, 2);
// Print a message to the LCD.
lcd.print("Te, amo, Nuriitaa!");
}

void loop() {
// set the cursor to column 0, line 1
// (note: line 1 is the second row, since counting begins with 0):
lcd.setCursor(0, 1);
// print the number of seconds since reset:
lcd.print(millis()/1000);
}
[/sourcecode]

Testing buttons
[sourcecode language="C"]
//Sample using LiquidCrystal library
#include <LiquidCrystal.h>

/*******************************************************

This program will test the LCD panel and the buttons
Mark Bramwell, July 2010

********************************************************/

// select the pins used on the LCD panel
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

// define some values used by the panel and buttons
int lcd_key = 0;
int adc_key_in = 0;
#define btnRIGHT 0
#define btnUP 1
#define btnDOWN 2
#define btnLEFT 3
#define btnSELECT 4
#define btnNONE 5

// read the buttons
int read_LCD_buttons()
{
adc_key_in = analogRead(0); // read the value from the sensor
// my buttons when read are centered at these valies: 0, 144, 329, 504, 741
// we add approx 50 to those values and check to see if we are close
if (adc_key_in > 1000) return btnNONE; // We make this the 1st option for speed reasons since it will be the most likely result
if (adc_key_in < 50) return btnRIGHT;
if (adc_key_in < 195) return btnUP;
if (adc_key_in < 380) return btnDOWN;
if (adc_key_in < 555) return btnLEFT;
if (adc_key_in < 790) return btnSELECT;
return btnNONE; // when all others fail, return this...
}

void setup()
{
lcd.begin(16, 2); // start the library
lcd.setCursor(0,0);
lcd.print("Push the buttons"); // print a simple message
}

void loop()
{
lcd.setCursor(9,1); // move cursor to second line "1" and 9 spaces over
lcd.print(millis()/1000); // display seconds elapsed since power-up


lcd.setCursor(0,1); // move to the begining of the second line
lcd_key = read_LCD_buttons(); // read the buttons

switch (lcd_key) // depending on which button was pushed, we perform an action
{
case btnRIGHT:
{
lcd.print("RIGHT ");
break;
}
case btnLEFT:
{
lcd.print("LEFT ");
break;
}
case btnUP:
{
lcd.print("UP ");
break;
}
case btnDOWN:
{
lcd.print("DOWN ");
break;
}
case btnSELECT:
{
lcd.print("SELECT");
break;
}
case btnNONE:
{
lcd.print("NONE ");
break;
}
}
}
[/sourcecode]

Robotis CM-900 as a tosser for Dynamixel commands

CM-900 is really tiny and cheap, so is perfect to use as communication bridge between any computer (Raspberry Pi, Pandaboard, etc. included, of course) and the Dynamixel bus. Whatever it receives from the Serial USB (usually commands and queries) is sent to the Dynamixel bus, and what it receives from the Dynamixel bus is sent to the SerialUSB (usually answers)

Here is the source code of the little program for CM-900 IDE:

[sourcecode language="C"]
int counter;
bool onlyOnceHappened;

void blinkOnce()
{
digitalWrite(BOARD_LED_PIN, LOW);
delay_us(100);
digitalWrite(BOARD_LED_PIN, HIGH);
}

void setup()
{
pinMode(BOARD_LED_PIN, OUTPUT);

onlyOnceHappened=false;
counter=0;

//USB Serial initialize
SerialUSB.begin();
// SerialUSB.attachInterrupt(USBDataReceived);
//DXL initialize
Dxl.begin(1);
}

byte aByte=0;
uint8 aUint8;

void loop()
{
// SerialUSB.println (counter++);

if (onlyOnceHappened==false)
{
blinkOnce();
onlyOnceHappened=true;
delay (3000); //Some time to the user to activate the monitor/console
SerialUSB.println ("v1.1.1 Orders receiver started");
}

if (SerialUSB.available())
{
aUint8=SerialUSB.read();
blinkOnce();
Dxl.writeRaw(aUint8);
// delay(20);
}

if (Dxl.available())
{
aByte=Dxl.readRaw();
blinkOnce();
SerialUSB.write(aByte);
// delay(20);
}
}

[/sourcecode]

Here the file.

In the next post I will include an improved version that could read sensors connected to the CM-900, "expanding" the Dynamixel protocol.

[UPDATED] A simple but pretty complete Linux C++ example for Bioloid

[Jun 12, 2013, Some source code typos fixed and code updated]:

.
.
.
.
.

[sourcecode language="cpp"]
bool nonValidOption(int option)
{
return (optionMaxOption);
}
[/sourcecode]

This is a very simple but pretty complete example, trying to use the least possible dependencies. I have tested in a just installed Ubuntu and you only needed the AXControl library (I created and use it because then there were no Robotis library and because it can use serial port, Dynamixel bus, Zigbee, BT (Robotis or other adaptors)

It use the terminal, no QT, to avoid library dependencies. But you should add the AXControl_v2 lib files to your /usr/lib folder as administrator (sudo). Click here if you want one unique executable for Linux (tested with Linux Mint 14) using these sources files

When executed, it shows the values configured in the HexaWheels.conf file and any problem it founds, like SerialPort2Dynamixel.open(ss)=> [Serial port NOT opened]:


ubuntu@A3820 ~/projects/trunk/bioloid/Herramientas/CommandLine $ ./commandline
init to load configuration(ss)=> [./HexaWheels.conf]
Configuration file=> (ss)=> [./HexaWheels.conf]
Tiempo_Espera_ms(ss)=> [35]
Tiempo_Espera_Sensores_ms(ss)=> [60]
Nombre_Puerto_Serie(ss)=> [/dev/ttyUSB0]
Baudios_Puerto_Serie(ss)=> [1000000]
Motion_File_Name(ss)=> [HexaWheels_2.0.mtn]
UmbralContacto(ss)=> [550]
UmbralDeteccion(ss)=> [200]
loadConfigurationValues: (si)=> [134529476]
getIntValue: (ss)=> [Tiempo_Espera_ms]
getIntValue: (ss)=> [Tiempo_Espera_Sensores_ms]
getIntValue: (ss)=> [Baudios_Puerto_Serie]
getStringValue: (ss)=> [Nombre_Puerto_Serie]

Device /dev/ttyUSB0, Speed B1000000
SerialPort2Dynamixel.open(ss)=> [Serial port NOT opened]
SerialPort2Dynamixel.open, serialPortName(ss)=> [/dev/ttyUSB0]
SerialPort2Dynamixel.open, baudRate(si)=> [4104]
I can't open DynamixelCommunication, name: (ss)=> [/dev/ttyUSB0]
terminate called after throwing an instance of 'std::exception*'
Aborted

If it can connect an there are no problem you should see:


ubuntu@A3820 ~/projects/trunk/bioloid/Herramientas/CommandLine $ ./commandline
init to load configuration(ss)=> [./HexaWheels.conf]
Configuration file=> (ss)=> [./HexaWheels.conf]
Tiempo_Espera_ms(ss)=> [35]
Tiempo_Espera_Sensores_ms(ss)=> [60]
Nombre_Puerto_Serie(ss)=> [/dev/ttyUSB0]
Baudios_Puerto_Serie(ss)=> [1000000]
Motion_File_Name(ss)=> [HexaWheels_2.0.mtn]
UmbralContacto(ss)=> [550]
UmbralDeteccion(ss)=> [200]
loadConfigurationValues: (si)=> [134529476]
getIntValue: (ss)=> [Tiempo_Espera_ms]
getIntValue: (ss)=> [Tiempo_Espera_Sensores_ms]
getIntValue: (ss)=> [Baudios_Puerto_Serie]
getStringValue: (ss)=> [Nombre_Puerto_Serie]

Device /dev/ttyUSB0, Speed B1000000
SerialPort2Dynamixel.open, serialPortName(ss)=> [/dev/ttyUSB0]
SerialPort2Dynamixel.open, baudRate(si)=> [4104]
V01
2.- Toggle AX-12 LED
3.- Query AX-12
4.- Move AX-12
5.- Torque AX-12
6.- Query CM-510 sensor
7.- Show menu
8.- Beep
9.- Quit
Select option (2-9)

[sourcecode language="cpp"]

/*------------------------------------------------------------------------------*\
* This source file is subject to the GPLv3 license that is bundled with this *
* package in the file COPYING. *
* It is also available through the world-wide-web at this URL: *
* http://www.gnu.org/licenses/gpl-3.0.txt *
* If you did not receive a copy of the license and are unable to obtain it *
* through the world-wide-web, please send an email to *
* siempre.aprendiendo@gmail.com so we can send you a copy immediately. *
* *
* @category Robotics *
* @copyright Copyright (c) 2011 Jose Cortes (<a href="https://plus.google.com/105007891378677151287/about">https://plus.google.com/105007891378677151287/about</a>) *
* @license http://www.gnu.org/licenses/gpl-3.0.txt GNU v3 Licence *
* *
\*------------------------------------------------------------------------------*/

#include "stdafx.h"
#include
#include "BasicSystem.h"
#include "Util.h"

using namespace std;

class UI
{
public:
static const int OptionQuit=8;

UI(MyBasicSystem::BasicSystem& aMySystem) : mySystem(aMySystem) {}

void showMenu();
int selectOption();
bool executeSelectedOption(int selected);
void doBeep();

private:
MyBasicSystem::BasicSystem mySystem;

static const int OptionSetLEDOnOff=1;
static const int OptionQueryAX12=2;
static const int OptionMoveAX12=3;
static const int OptionTorqueAX12=4;
static const int OptionQuerySensor=5;
static const int OptionShowMenu=6;
static const int OptionBeep=7;

static const int MinOption=OptionSetLEDOnOff;
static const int MaxOption=OptionQuit;

bool validRange(int value, int MinValue, int MaxValue);
bool nonValidOption(int option);
int getPosition();
int getId();
int getSensorPort();
void doGetPosition();
void doSetPosition();
void doQuerySensor();
void doSetLEDOnOff();
void doSetTorqueOnOff();

};

void UI::showMenu()
{
cout << "V02" << endl;

cout << OptionSetLEDOnOff << ".- Toggle AX-12 LED" << endl;
cout << OptionQueryAX12 << ".- Query AX-12" << endl;
cout << OptionMoveAX12 << ".- Move AX-12" << endl;
cout << OptionTorqueAX12 << ".- Torque AX-12" << endl;
cout << OptionQuerySensor << ".- Query CM-510 sensor" << endl;
cout << OptionShowMenu << ".- Show menu" << endl;
cout << OptionBeep << ".- Beep" << endl;
cout << OptionQuit << ".- Quit" << endl;
}

bool UI::nonValidOption(int option)
{
return (optionMaxOption);
}

int UI::selectOption()
{
int option=-1;

while (nonValidOption(option))
{
cout << "Select option (" << MinOption << "-" << MaxOption << ")" << endl; cin >> option;
if (nonValidOption(option))
{
cout << endl;
cout << endl;
cout << "(" << option << ") is NOT a valid option" << endl; } } return option; } bool UI::validRange(int value, int MinValue, int MaxValue) { return (value>=MinValue && value<=MaxValue);
}

int UI::getPosition()
{
int position=0;

do
{
cout << "Type a value between 0 and 1023 to set the AX-12 in that position" << endl; cin>>position;

}while(!validRange(position, 0, 1023));

cout << "Position:" << position << endl; return position; } int UI::getId() { int ax12Id=1; do { puts ("Type the ID of the AX-12 to use, a value between 1 and 18, "); cin >> ax12Id;

}while(!validRange(ax12Id, 1, 18));

cout << "AX12 Id:" << ax12Id << endl; return ax12Id; } int UI::getSensorPort() { int sensorPort=1; do { puts ("Type the sensor Port to read, a value between 1 and 6, "); cin >> sensorPort;

}while(!validRange(sensorPort, 1, 6));

cout << "Sensor Port number:" << sensorPort << endl;

return sensorPort;
}

void UI::doBeep()
{
cout << "Beep" << endl;
mySystem.dynamixelCommunication.sendOrder(100, AXS1_Buzzer, (byte) DO, (short) 500);
Sleep (2);
mySystem.dynamixelCommunication.sendOrder(200, MyCommunications::Beep, (byte) 5);
}

void UI::doGetPosition()
{
int ax12Id=getId();

int position = mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::PresentPosition);
cout << "the position is: [" << position << "] " << endl << endl << endl;
}

void UI::doSetPosition()
{
int ax12Id=getId();
int position=getPosition();

mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::GoalPosition,(short) position);

}

void UI::doQuerySensor()
{
int sensorPort=getSensorPort();

int value=mySystem.dynamixelCommunication.readSensorValue (200, ReadCM510SensorRaw, sensorPort);
cout << "the sensor reads: [" << value << "] " << endl << endl << endl;
}

void UI::doSetLEDOnOff()
{
byte lByte=0, hByte=0;
int ledValue;

int ax12Id=getId();

ledValue=mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::LED);
Hex::toHexHLConversion(ledValue, hByte, lByte);

bool onOff=false;
if (lByte!=0)
onOff=true;

cout << "The LED is: [" << (onOff?"on":"off") << "], putting it: [" << (onOff?"Off":"on") << "] " << endl << endl << endl;
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::LED, byte(onOff?0:1));
}

void UI::doSetTorqueOnOff()
{
byte lByte=0, hByte=0;
int ax12Id=getId();

int torque=mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::Torque);
Hex::toHexHLConversion(torque, hByte, lByte);
bool onOff=(lByte!=0?true:false);

cout << "The Torque is: [" << (onOff?"on":"off") << "], putting it: [" << (onOff?"Off":"on") << "] " << endl << endl << endl;

mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::Torque,(byte) (onOff?0:1));
}

bool UI::executeSelectedOption(int option)
{
bool isOk=true;
cout << endl;
cout << endl;

cout << "Selected option: [" << option << "] " << endl;

switch(option)
{
case OptionSetLEDOnOff:
doSetLEDOnOff();
break;
case OptionQueryAX12:
doGetPosition();
break;
case OptionMoveAX12:
doSetPosition();
break;
case OptionTorqueAX12:
doSetTorqueOnOff();
break;
case OptionQuerySensor:
doQuerySensor();
break;
case OptionShowMenu:
showMenu();
break;
case OptionBeep:
doBeep();
break;
}

return isOk;
}

int _tmain(int argc, _TCHAR* argv[])
{
cout << "AXControl_v2_VS_CPP test v0" << endl;

bool quit=false;
MyBasicSystem::BasicSystem mySystem;
UI ui(mySystem);
do
{
ui.showMenu();
int selected=ui.selectOption();
if (selected==ui.OptionQuit)
quit=true;
ui.executeSelectedOption(selected);
}while (!quit);

mySystem.dynamixelCommunication.close();

return 0;
}

[/sourcecode]

Cheapest Smart Car Robot: parts, examples of use and documentation

[caption id="attachment_1552" align="aligncenter" width="600"] Ultrasonic Smart Car Kit[/caption]

[caption id="attachment_1746" align="alignleft" width="150"] Funduino Arduino DuemilaNove clon[/caption]

It's really cheap but it does not include any documentation; but don't panic, all you will need can be found on Internet. And now, the main electronic components: the microcontroller, the dual motor driver, the ultrasonic sensor and the servo:

The microcontroller brain is a Funduino (Arduino Duemilanove clone), and as far I've used it (and as other buyer said is fully, at least for software, compatible). Here we will not have any problem because there are a lot of documentation about Arduino and Duemilanove.

A simple hello world example (here the file)

[sourcecode language="c"]

// Blinking LED

int ledPin = 13;                 // LED connected to digital pin 13

void setup()
{
Serial.begin(57600);
printf (&amp;amp;quot;Setup/n/l&amp;amp;quot;);
Serial.println(&amp;amp;quot;Hello world!&amp;amp;quot;);
pinMode(ledPin, OUTPUT);      // sets the digital pin as output
}

void loop()
{
printf (&amp;amp;quot;2009&amp;amp;quot;);
Serial.println(&amp;amp;quot;2009&amp;amp;quot;);
digitalWrite(ledPin, HIGH);   // sets the LED on
delay(1000);                  // waits for a second
digitalWrite(ledPin, LOW);    // sets the LED off
delay(1000);                  // waits for a second
}

[/sourcecode]

[caption id="attachment_1748" align="alignleft" width="150"] Dual_H-Bridge_Motor_Driver[/caption]

But to control the two motors this kit use an Dual H-Bridge motor driver (L298). Here (read the Drive Two DC Motors section) you can find a lot of useful documentation and examples,  and  here you can find also more information, specially about electronics.

Here the file with the example

[sourcecode language="c"]
int ENA=5;//connected to Arduino's port 5(output pwm)
int IN1=2;//connected to Arduino's port 2
int IN2=3;//connected to Arduino's port 3
int ENB=6;//connected to Arduino's port 6(output pwm)
int IN3=4;//connected to Arduino's port 4
int IN4=7;//connected to Arduino's port 7
void setup()
{
Serial.begin(57600);
Serial.println(&amp;amp;quot;Two motors&amp;amp;quot;);
pinMode(ENA,OUTPUT);//output
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);//stop driving
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);//setting motorA's directon
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);//setting motorB's directon
}
void loop()
{
analogWrite(ENA,255);//start driving motorA
analogWrite(ENB,255);//start driving motorB
delay(3000);
analogWrite (IN1,0);//stop driving motorA
analogWrite (IN2,0);//stop driving motorB

analogWrite (IN3,0);//stop driving motorA
analogWrite (IN4,0);//stop driving motorB

delay (3000);

int ledPin = 13;
while (1)
{
digitalWrite(ledPin, HIGH);   // sets the LED on
delay(1000);                  // waits for a second
digitalWrite(ledPin, LOW);    // sets the LED off
delay(000);
}
&amp;amp;lt;pre&amp;amp;gt;[/sourcecode]

[caption id="attachment_1753" align="alignleft" width="150"] Ultrasonic sensor hc-sr04[/caption]

The HC-SR04 ultrasonic distance sensor, example of use (here the file):

[sourcecode language="c"]

#define trigPin 9
#define echoPin 11

void setup() {
Serial.begin (115200);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}

void loop() {
int duration, distance;
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance &amp;amp;gt; 400 || distance &amp;amp;lt; 0){
Serial.println(&amp;amp;quot;Out of range&amp;amp;quot;);
}
else {
Serial.print(distance);
Serial.println(&amp;amp;quot; cm&amp;amp;quot;);
}
delay(500);
}

[/sourcecode]

[caption id="attachment_1755" align="alignleft" width="150"] TowerPro micro servo SG90[/caption]

And the Tower PRO SG-90 micro servo (and here the file with the example):

[sourcecode language="c"]
// Sweep
// by BARRAGAN &amp;amp;lt;http://barraganstudio.com&amp;amp;gt;
// This example code is in the public domain.

#include &amp;amp;lt;Servo.h&amp;amp;gt;

Servo myservo;  // create servo object to control a servo
// a maximum of eight servo objects can be created

int pos = 0;    // variable to store the servo position

void setup()
{
myservo.attach(3);  // attaches the servo on pin 3 to the servo object
}

void loop()
{
for(pos = 0; pos &amp;amp;lt; 180; pos += 1)  // goes from 0 degrees to 180 degrees
{                                  // in steps of 1 degree
myservo.write(pos);              // tell servo to go to position in variable 'pos'
delay(15);                       // waits 15ms for the servo to reach the position
}
for(pos = 180; pos&amp;amp;gt;=1; pos-=1)     // goes from 180 degrees to 0 degrees
{
myservo.write(pos);              // tell servo to go to position in variable 'pos'
delay(15);                       // waits 15ms for the servo to reach the position
}
}
[/sourcecode]

Soon I will add an LCD display with 6 buttons

CM-900 Robotis IDE more programming examples

I've started to program CM-900 "translating" the example from the C programming tutorial for CM-510



You can get more information at robotsource.org CM-900 community circle and the CM-900 e-manual. There you can find:

- Quick start guide

- Presentation Workshop

- Arduino based IDE to program it very easily (examples included)

- Source code and instructions

- Using other servos

- Interfacing sensors

I already have "translated" two examples: a simple "hello world" like where a servo is move to 2 different positions and another where the servo id and position is asked and after entered the data the servo is positionated in the typed position, validating that id and position are in range.

The "hello World " example (here the zipped file):

[sourcecode language="c"]
#define P_GOAL_POSITION_L     30
#define P_MOVING              46

int counter;
int onlyOnceHappened;

void setup()
{
//USB Serial initialize

onlyOnceHappened=0;
counter=0;

SerialUSB.begin();
Dxl.begin(1);
delay (5000);
SerialUSB.println("Setup");
}

void loop()
{

delay (3000);
SerialUSB.print("loop");
SerialUSB.println(counter);
counter++;
delay(1000);

if (onlyOnceHappened==0)
{
onlyOnceHappened=1;
SerialUSB.println("Hello, World");
}

int id=8;

SerialUSB.println ("Simple example #0");

SerialUSB.print ("Perform movement 1 with the AX-12:");
SerialUSB.println (id);
byte bMoving = Dxl.readByte( id, P_MOVING);
byte CommStatus = Dxl.getResult();
if( CommStatus == COMM_RXSUCCESS )
Dxl.writeWord( id, P_GOAL_POSITION_L, 511 );
else
SerialUSB.println ("CommStatus IS NOT COMM_RXSUCCESS");

SerialUSB.println ("Pause half a second");
delay (500); // half-second pause

SerialUSB.println ("Beep!");
//buzzOn (100); // beep

SerialUSB.println ("Pause for a second");
delay (1000); // pause for 1 second

SerialUSB.print ("Perform movement 2 with the AX-12: ");
SerialUSB.println (id);

Dxl.writeWord( id, P_GOAL_POSITION_L, 611 );

SerialUSB.println ("End");

}
[/sourcecode]

Asking Id and Position example, (here the zipped file)

[sourcecode language="c"]

int counter;
bool onlyOnceHappened;

int id;
int position;

bool debugOutputOn;
char charReaded;

void debugOutputUSB(char *buffer, bool newlineAfterMessage)
{
if (debugOutputOn)
{
if (newlineAfterMessage)
SerialUSB.println(buffer);
else
SerialUSB.print(buffer);
}
}

void debugOutputUSB(int value, bool newlineAfterMessage)
{
if (debugOutputOn)
{
if (newlineAfterMessage)
SerialUSB.println(value);
else
SerialUSB.print(value);
}
}

void debugOutputPairUSB(char *buffer, int value)
{
if (debugOutputOn)
{
SerialUSB.println ("");
SerialUSB.print(buffer);
SerialUSB.println(value);
}
}

void debugOutputPairUSB(char *buffer1, char *buffer2)
{
if (debugOutputOn)
{
SerialUSB.println ("");
SerialUSB.print(buffer1);
SerialUSB.println(buffer2);
}
}

void setup()
{
//USB Serial initialize

onlyOnceHappened=false;
counter=0;
debugOutputOn=false;

SerialUSB.begin();
Dxl.begin(1);
delay (3000);

debugOutputUSB("Setuped", true);
}

void checkIfDebugOn()
{
SerialUSB.print("Put debug ON?, y/n");
charReaded=SerialUSB.read();

if (charReaded=='y' || charReaded=='Y')
debugOutputOn=true;
else
debugOutputOn=false;

debugOutputPairUSB ("DebugPut is:", debugOutputOn);
}

void loop()
{
delay (1500);
SerialUSB.println ("Simple example #1");

if (onlyOnceHappened==false)
{
onlyOnceHappened=true;
checkIfDebugOn();
}

debugOutputPairUSB  ("Loop: ",counter);
counter++;
delay(500);

debugOutputUSB("Starting loop", true);

while(true) // we'll repeat this loop forever
{
id=getId(); // get the ID of the AX-12 that we want to move
position=getPosition(); // get the goal position

Dxl.writeWord( id, P_GOAL_POSITION_L, position); // sent the command to the Dynamixel
debugOutputUSB ("Dxl.writeWorded", true);
}
}
// I have created a simple ayoi because I have problem compiling atoi
int myatoi(char *buffer)
{
int len=strlen(buffer)-2;
int value=0;
int i=0;
int base=0;
int number=0;

for (i=len;i>=0;i--)
{
number=int(buffer[i])-int('0');

debugOutputPairUSB ("buffer[i]: ", int(buffer[i]));

if (i==len)
{
value=number;
}
else
{
value+=base * number;
}

debugOutputPairUSB ("i: ",i);
debugOutputPairUSB ("number: ", number);
debugOutputPairUSB("base: ", base);
debugOutputPairUSB("valor: ", value);

if (base==0)
{
base=10;
}
else
base=base*10;
}

return value;
}

bool isValid(int value, int MinValue, int MaxValue)
{
return (value>=MinValue && value<=MaxValue);
}

void readString(char *bufferParameter)
{
int i=0; // We'll use this variable as index of the buffer where we will store data

do
{
bufferParameter[i]=(char) SerialUSB.read(); // it store the read character in the i position of bufferParameter
SerialUSB.print(bufferParameter[i]); // showing it
if (bufferParameter[i]=='\b') // if Backspace was pressed
i--; // it goes to the previous position, rewritting the previoulsy typed character
else //
i++; // it will write in the next position
//    SerialUSB.println(int(bufferParameter[i-1]));
}while(bufferParameter[i-1]!=10); // while the last values is not INTRO. The symmbol ! represents the logical operator NOT

bufferParameter[i]=0; // A NULL (0, zero) is necessary in the last position of any string
}

/*
It read an string, it's converted to integer and returned
*/
int readInteger(char *bufferParameter)
{
//  return SerialUSB.parseInt();

readString(bufferParameter);

//  int valor=int(bufferParameter[0]-int('0'));
debugOutputPairUSB ("Leido: ", bufferParameter);
int valor=myatoi(bufferParameter);

debugOutputPairUSB ("Valor leido: ", valor);

return valor;
}

int getId()
{
/*
We define an array enough large, 256 bytes (characters). Probably it's enough with 4 bytes, but if we type more than the defined size we will get an error*/
char buffer[256];

/*
And we define another integer variable, it's very advisable to asign a value in the definition, in this case we assign the minimun value, 1*/
int ax12Id=1;

do
{ // starting the loop
SerialUSB.println ("Enter the ID of the AX-12 that you wwant to move, between 1 y 18: ");
SerialUSB.print("Id:");
ax12Id=readInteger(buffer); // this function will read from what we type in the keyboard
//// exclamation (!) is the NOT logical operator. It will repeat the loop while the value is not valid
}while(!isValid(ax12Id, 1, 18));

// Showing the typed value
SerialUSB.println(" ");
SerialUSB.print("AX12 ID:");
SerialUSB.println(ax12Id);

return ax12Id;
}

int getPosition()
{
char buffer[256];
int position=0;

do
{
SerialUSB.println ("Enter a value between 0 and 1023 as the goal position");
SerialUSB.print ("Position:");
position=readInteger(buffer);
}while(!isValid(position, 0, 1023));

// Showing the typed value
SerialUSB.println(" ");
SerialUSB.print("Position:");
SerialUSB.println(position);

return position;
}

[/sourcecode]