ok....
I am on the very beginning of this project, so the arduino project is quite simple right now. I copied mostly from other samples.
The PC program has a simple GUI for some mode selection, reading buttons and reading joystick X and Y via HID (Human Interface Device) SDK.
All this values are sent in serial ASCII telegram (14 Bytes only) cyclically each 250 ms.
The Arduino sketch is controlling 2 DC motors (motor shield V1), depending on the information of serial cyclic telegram. Right now there is no information send back to PC!
Later on, when basic communication is working, the sketch will be expanded by Stepper control and more complex calculations
Arduino sketch:
#include <Servo.h>
#include <AFMotor.h>
#define LINE_BUFFER_LENGTH 1024
#define SPEED_MIN 60
#define SPEED_MAX 255
#define RAMP_STEP 2
AF_DCMotor Motor1(1,MOTOR12_1KHZ);
AF_DCMotor Motor2(2,MOTOR12_1KHZ);
// Set to true to get debug output.
boolean verbose = false;
bool button1 = false;
bool button2 = false;
bool button3 = false;
int JoyX = 0;
int JoyY = 0;
int SpeedHeel = 0;
int SpeedKnee = 0;
int SpeedHip = 0;
int mode = 0; // off
typedef struct
{
int n;
AF_DCMotor* pdrv;
int speed_act;
int speed_soll;
}m_t;
m_t Heel = { 1, &Motor1,0,0 };
m_t Knee = { 2, &Motor2,0,0 };
/**********************
- void setup() - Initialisations
***********************/
void setup() {
// Setup
Serial.begin( 115200 );
// Notifications!!!
Serial.println("Control Humanoid alive and kicking!");
Motor1.setSpeed(0);
Motor1.run(RELEASE);
Motor2.setSpeed(0);
Motor2.run(RELEASE);
}
/**********************
- void loop() - Main loop
***********************/
void loop()
{
char line[ LINE_BUFFER_LENGTH ];
char c;
int lineIndex;
bool lineIsComment, lineSemiColon;
lineIndex = 0;
lineSemiColon = false;
lineIsComment = false;
while (1)
{
// Serial reception - Mostly from Grbl, added semicolon support
while ( Serial.available()>0 )
{
Serial.println( "getc" );
c = Serial.read();
if (( c == '\n') || (c == '\r') ) { // End of line reached
if ( lineIndex > 0 ) { // Line is complete. Then execute!
line[ lineIndex ] = '\0'; // Terminate string
if (verbose) {
Serial.print( "Received : ");
Serial.println( line );
}
processIncomingLine( line, lineIndex );
lineIndex = 0;
}
else {
// Empty or comment line. Skip block.
}
lineIsComment = false;
lineSemiColon = false;
Serial.println("ok");
}
else {
if ( (lineIsComment) || (lineSemiColon) ) { // Throw away all comment characters
if ( c == ')' ) lineIsComment = false; // End of comment. Resume line.
}
else {
if ( c <= ' ' ) { // Throw away whitepace and control characters
}
else if ( c == '/' ) { // Block delete not supported. Ignore character.
}
else if ( c == '(' ) { // Enable comments flag and ignore all characters until ')' or EOL.
lineIsComment = true;
}
else if ( c == ';' ) {
lineSemiColon = true;
}
else if ( lineIndex >= LINE_BUFFER_LENGTH-1 ) {
Serial.println( "ERROR - lineBuffer overflow" );
lineIsComment = false;
lineSemiColon = false;
}
else if ( c >= 'a' && c <= 'z' ) { // Upcase lowercase
line[ lineIndex++ ] = c-'a'+'A';
}
else {
line[ lineIndex++ ] = c;
}
}
}
}
switch(mode)
{
case 1:
SpeedHeel = (button1)?JoyY:0;
SpeedKnee = (button2)?JoyY:0;
SpeedHip = (button3)?JoyY:0;
break;
case 2:
SpeedHeel = JoyY;
SpeedKnee = JoyY;
SpeedHip = JoyY;
break;
case 0: // do nothing
default:
int SpeedHeel = 0;
int SpeedKnee = 0;
int SpeedHip = 0;
break;
}
if (verbose)
{
Serial.print( "Mode:" );
Serial.print( mode, DEC );
Serial.print( " / Speed Heel:" );
Serial.print( SpeedHeel, DEC );
Serial.print( " / Speed Knee:" );
Serial.println( SpeedKnee, DEC );
}
Heel.speed_soll = SpeedHeel;
Knee.speed_soll = SpeedKnee;
ctrl_motor(&Heel);
ctrl_motor(&Knee);
}
}
void processIncomingLine( char* line, int charNB )
{
int currentIndex = 0;
char buffer[ 128 ]; // Hope that 64 is enough for 1 parameter
switch ( line[ currentIndex++ ] )
{ // Select command, if any
case 'I': // Init/Configuration Mode
mode = 1;
break;
case 'O': // Operational
mode = 2;
break;
default:
mode = 0;
break;
}
int tmp;
char delimiter;
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[1] = '\0';
tmp = atoi( buffer );
button1 = tmp!=0;
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[1] = '\0';
tmp = atoi( buffer );
button2 = tmp!=0;
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[1] = '\0';
tmp = atoi( buffer );
button3 = tmp!=0;
delimiter = line[ currentIndex++ ];
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[1] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[2] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[3] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[4] = '\0';
JoyX = atoi( buffer );
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[1] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[2] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[3] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
buffer[4] = '\0';
JoyY = atoi( buffer );
}
void ctrl_motor(m_t* pm)
{
if (verbose) {
Serial.print("M");
Serial.print(pm->n, DEC);
}
if ( pm->speed_act < pm->speed_soll )
{
if (verbose)
{
Serial.println("Ramp up");
}
pm->speed_act += RAMP_STEP;
if ( pm->speed_act > pm->speed_soll )
{
pm->speed_act = pm->speed_soll;
}
}
else if ( pm->speed_act > pm->speed_soll )
{
if (verbose)
{
Serial.println("Ramp down");
}
pm->speed_act -= RAMP_STEP;
if ( pm->speed_act < pm->speed_soll )
{
pm->speed_act = pm->speed_soll;
}
}
if ( pm->speed_act > SPEED_MIN )
{
if (verbose)
{
Serial.print(": forward:");
Serial.print(pm->speed_act, DEC);
}
pm->pdrv->setSpeed(pm->speed_act);
pm->pdrv->run(FORWARD);
}
else if ( pm->speed_act < (-SPEED_MIN) )
{
if (verbose)
{
Serial.print(": backward:");
Serial.print(pm->speed_act, DEC);
}
pm->pdrv->setSpeed(abs(pm->speed_act));
pm->pdrv->run(BACKWARD);
}
else
{
pm->pdrv->setSpeed(0);
pm->pdrv->run(RELEASE);
}
if (verbose)
{
Serial.println("...done");
}
}
for a few seconds the serial communication works fine, but then it stucks!
