Real-time RoboNova PC Interface

Hitec robotics including ROBONOVA humanoid, HSR-8498HB servos, MR C-3024 Controllers and RoboBasic
16 postsPage 1 of 21, 2
16 postsPage 1 of 21, 2

Real-time RoboNova PC Interface

Post by roboninja » Thu Jun 25, 2009 1:25 am

Post by roboninja
Thu Jun 25, 2009 1:25 am

Hi,
I'm working on a software interface to control the RoboNova in real-time and I was wondering if anyone on the forum has done this and what their experiences are.

Basically my ultimate goal is to have a program running on my PC that will generate a walking gait in real-time, based on sensor feedback, and then send this motion data point-by-point to the RoboNova via the serial connection. I don't want any preset "gaits," it will all be generated on the fly. At this point I'm working on coding the interface library that will stand between my real-time gait generation software and the RoboNova.

From my understanding all I need to do is connect my PC to the RoboNova via my serial port and send out serial commands detailed in this document:
http://web.ukonline.co.uk/r.ibbotson/files/C3024Serial.pdf

Is this correct?

I have gotten this functionality working on a KHR-2 but any thoughts, ideas, suggestions on how to best get this working on a RoboNova would be GREATLY appreciated!

Thanks!
Hi,
I'm working on a software interface to control the RoboNova in real-time and I was wondering if anyone on the forum has done this and what their experiences are.

Basically my ultimate goal is to have a program running on my PC that will generate a walking gait in real-time, based on sensor feedback, and then send this motion data point-by-point to the RoboNova via the serial connection. I don't want any preset "gaits," it will all be generated on the fly. At this point I'm working on coding the interface library that will stand between my real-time gait generation software and the RoboNova.

From my understanding all I need to do is connect my PC to the RoboNova via my serial port and send out serial commands detailed in this document:
http://web.ukonline.co.uk/r.ibbotson/files/C3024Serial.pdf

Is this correct?

I have gotten this functionality working on a KHR-2 but any thoughts, ideas, suggestions on how to best get this working on a RoboNova would be GREATLY appreciated!

Thanks!
roboninja offline
Newbie
Newbie
Posts: 6
Joined: Thu Jun 25, 2009 1:08 am

Post by i-Bot » Thu Jun 25, 2009 9:37 am

Post by i-Bot
Thu Jun 25, 2009 9:37 am

There are some posts earlier in this forum from people who have done this.

The challenge is to get it to work fast enough. The serial link is a 9600 and the protocol is a ping pong with the PC and RoboNova exchanging one byte at a time. The reason for this protocol is that the UART on the RoboNova is not interrupt driven and has limited buffer. The RoboNova Processor is busy managing the servos or other activity for long periods.

To get best performance you may need to:
1) Make any PC adjustments to USB or serial to maximise performance on single byte messages. Bluetooth seems to make this worse.

2) Stop the Robobasic interpreter in the RoboNova

3) Take care in using other serial instructions which take a long time ( reading servo positions)

4) Use Point to Point on RoboNova to interpolate between poses

I did raise the serial speed to 115K, but the latency meant the improvement was small.

I would expect you should be able to make a sync move about every 100mSec.

Some errors in my original document have also been described and corrected in this forum.
There are some posts earlier in this forum from people who have done this.

The challenge is to get it to work fast enough. The serial link is a 9600 and the protocol is a ping pong with the PC and RoboNova exchanging one byte at a time. The reason for this protocol is that the UART on the RoboNova is not interrupt driven and has limited buffer. The RoboNova Processor is busy managing the servos or other activity for long periods.

To get best performance you may need to:
1) Make any PC adjustments to USB or serial to maximise performance on single byte messages. Bluetooth seems to make this worse.

2) Stop the Robobasic interpreter in the RoboNova

3) Take care in using other serial instructions which take a long time ( reading servo positions)

4) Use Point to Point on RoboNova to interpolate between poses

I did raise the serial speed to 115K, but the latency meant the improvement was small.

I would expect you should be able to make a sync move about every 100mSec.

Some errors in my original document have also been described and corrected in this forum.
i-Bot offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 1142
Joined: Wed May 17, 2006 1:00 am

Alternative Controller?

Post by roboninja » Thu Jun 25, 2009 11:28 pm

Post by roboninja
Thu Jun 25, 2009 11:28 pm

I read some older posts on the forum about an open source controller as an alternative to the MR-C3024 but it didn't look like anything ever came of it. Do you know if anyone has successfully replaced the C3024 with something that I might be able to communicate with a bit quicker?

I read on another forum that, for instance, the Lynxmotion SSC-32 servo controller will work with the Hitec HSR-8498 servos so perhaps that could be a possibility. Think that might be a valid route to take?

Thanks again!
I read some older posts on the forum about an open source controller as an alternative to the MR-C3024 but it didn't look like anything ever came of it. Do you know if anyone has successfully replaced the C3024 with something that I might be able to communicate with a bit quicker?

I read on another forum that, for instance, the Lynxmotion SSC-32 servo controller will work with the Hitec HSR-8498 servos so perhaps that could be a possibility. Think that might be a valid route to take?

Thanks again!
roboninja offline
Newbie
Newbie
Posts: 6
Joined: Thu Jun 25, 2009 1:08 am

Post by limor » Fri Jun 26, 2009 11:10 am

Post by limor
Fri Jun 26, 2009 11:10 am

Check out the new Ro-Board. It puts the PC on the robot so to speak.
designed specifically for multi-servo humanoids and closed-loop control.
Not sure about size compatibility but i'm sure someone will be able to provide those measurements.
http://robosavvy.com/forum/viewforum.php?f=17
Check out the new Ro-Board. It puts the PC on the robot so to speak.
designed specifically for multi-servo humanoids and closed-loop control.
Not sure about size compatibility but i'm sure someone will be able to provide those measurements.
http://robosavvy.com/forum/viewforum.php?f=17
limor offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 1845
Joined: Mon Oct 11, 2004 1:00 am
Location: London, UK

Post by PedroR » Fri Jun 26, 2009 2:26 pm

Post by PedroR
Fri Jun 26, 2009 2:26 pm

Hi roboninja

Depending on your programming skills I believe the Roboard will pose a great alternative for your robonova.

It is a bit larger than the original Robonova controller board (the size of the robosard is similar to the size of a credit card; only a bit larger but the height is similar). If you check out the roboard docs here http://robosavvy.com/site/index.php?opt ... Itemid=135 you get the full specs.

The board has 24 PWM interfaces so it should be compatible with the Robonova servos.
In the case of robonova it is a drop in replacement: you connect the Robonova battery to the roboard and connect the servos and the board will power the servos and do all the PWM communication.

However - and this is where programming skill is needed - you will need to program the board for your own usage.
The board is a full PC so it can run Windows XP and Linux and you can use Visual Studio or gcc to program it.
There is also an open source library that encapsulates all the PWM functions and communication. Additionally you can send RAW PWM pulses (although this is not really needed as the library implements the most common functions).
The library also implements techniques to capture static postures (reading the servo position through PWM instructions) and to capture dynamic movements.

The capture of Dynamic movements is quite unique: instead of capturing static poses and then animating them, you can capture the full fluid motion and replay it later.

If you need further technical information Please let me know.
The page here http://robosavvy.com/site/index.php?opt ... ce_Library includes a link to a full description of the Software Library features.
Hi roboninja

Depending on your programming skills I believe the Roboard will pose a great alternative for your robonova.

It is a bit larger than the original Robonova controller board (the size of the robosard is similar to the size of a credit card; only a bit larger but the height is similar). If you check out the roboard docs here http://robosavvy.com/site/index.php?opt ... Itemid=135 you get the full specs.

The board has 24 PWM interfaces so it should be compatible with the Robonova servos.
In the case of robonova it is a drop in replacement: you connect the Robonova battery to the roboard and connect the servos and the board will power the servos and do all the PWM communication.

However - and this is where programming skill is needed - you will need to program the board for your own usage.
The board is a full PC so it can run Windows XP and Linux and you can use Visual Studio or gcc to program it.
There is also an open source library that encapsulates all the PWM functions and communication. Additionally you can send RAW PWM pulses (although this is not really needed as the library implements the most common functions).
The library also implements techniques to capture static postures (reading the servo position through PWM instructions) and to capture dynamic movements.

The capture of Dynamic movements is quite unique: instead of capturing static poses and then animating them, you can capture the full fluid motion and replay it later.

If you need further technical information Please let me know.
The page here http://robosavvy.com/site/index.php?opt ... ce_Library includes a link to a full description of the Software Library features.
PedroR offline
Savvy Roboteer
Savvy Roboteer
Posts: 1199
Joined: Mon Jun 16, 2008 11:07 pm

Post by roboTT » Tue Jul 07, 2009 10:28 pm

Post by roboTT
Tue Jul 07, 2009 10:28 pm

I wrote a W32 application that was talking to RN1 MRC board using serial - in clear C.

It was really working fine, i could read sensor data, speak to servos...

But?
The problem was the cable running around with the bot so i dropped that.


Ill try to find my code somewhere and i'll post it.
I wrote a W32 application that was talking to RN1 MRC board using serial - in clear C.

It was really working fine, i could read sensor data, speak to servos...

But?
The problem was the cable running around with the bot so i dropped that.


Ill try to find my code somewhere and i'll post it.
roboTT offline
Savvy Roboteer
Savvy Roboteer
Posts: 62
Joined: Mon Mar 10, 2008 10:06 am

Post by PedroR » Wed Jul 08, 2009 1:13 pm

Post by PedroR
Wed Jul 08, 2009 1:13 pm

Would 't it be possible to replace the cable with a bluetooth or zigbee modem for wireless freedom?

Something like Bluesmirf that offers transparent replacement for serial comm?
Would 't it be possible to replace the cable with a bluetooth or zigbee modem for wireless freedom?

Something like Bluesmirf that offers transparent replacement for serial comm?
PedroR offline
Savvy Roboteer
Savvy Roboteer
Posts: 1199
Joined: Mon Jun 16, 2008 11:07 pm

Post by Hephaestus » Fri Jul 10, 2009 9:43 pm

Post by Hephaestus
Fri Jul 10, 2009 9:43 pm

this could get costly but
http://www.sparkfun.com/commerce/produc ... ts_id=9014
connected to an arduino and wireless com via bluesmirf or other radio tech would eliminate the current controller

my current setup with the blueSmirf - as you should know - only replaces the IR remote - no comm between
this could get costly but
http://www.sparkfun.com/commerce/produc ... ts_id=9014
connected to an arduino and wireless com via bluesmirf or other radio tech would eliminate the current controller

my current setup with the blueSmirf - as you should know - only replaces the IR remote - no comm between
Hephaestus offline
Newbie
Newbie
Posts: 5
Joined: Sun Jan 20, 2008 4:46 pm

Post by Hephaestus » Sat Jul 11, 2009 1:12 am

Post by Hephaestus
Sat Jul 11, 2009 1:12 am

OOOPPPSSS should have read a bit more - only works with standard servos - but the idea is a winner.
OOOPPPSSS should have read a bit more - only works with standard servos - but the idea is a winner.
Hephaestus offline
Newbie
Newbie
Posts: 5
Joined: Sun Jan 20, 2008 4:46 pm

Post by roboninja » Mon Jul 20, 2009 3:48 am

Post by roboninja
Mon Jul 20, 2009 3:48 am

The RoBoard looks like a pretty sweet solution. I'll have to pick one up and play with it a bit. I'll keep you guys posted
The RoBoard looks like a pretty sweet solution. I'll have to pick one up and play with it a bit. I'll keep you guys posted
roboninja offline
Newbie
Newbie
Posts: 6
Joined: Thu Jun 25, 2009 1:08 am

Post by roboTT » Wed Aug 12, 2009 7:32 pm

Post by roboTT
Wed Aug 12, 2009 7:32 pm

Here is my serial code, that drives my RN1 board directly from W32 C++ application.


Code: Select all
         //max odleglosc sensora IR

HANDLE hThread_networking ;
DWORD  threadId_networking ;
HANDLE hCom;
char sts[128];
void Servo_readposition(int servo);   
void Servo_syncmove(int ptp,int firstservo,int lastservo,int value);
void SerialPutc(HANDLE hCom, char txchar);
int SerialGetc(HANDLE hCom);
int IR_getdata();
float IR_centimeter(int wartosc);
void Servo_enable(int which);
void Servo_move(int which,int position);
void Servo_setspeed(int speed);
void AD_getdata(int port);
void Servo_readgroup6A();
void Servo_readgroup6B();
void Servo_readgroup6C();
void Servo_readgroup6D();
int Memory_read(int low,int high);

DWORD WINAPI THREAD_listenpackets(void *parm);
void Servo_wait(int ktore,int wartosc){
   int a=0;
   while(ROBONOVA.servo[ktore].value!=wartosc){
      if(GetTickCount()-tick>10){
         a=Memory_read(32+ktore,0x03);
         ROBONOVA.servo[ktore].value=a;
         tick=GetTickCount();
      }
   }
}

float Voltage(){
int value;
AD_getdata(5);
return (10*ROBONOVA.sensor[5].value)/256;
}
int Memory_read(int low,int high){
   int a;
   SerialPutc(hCom,0xF7);SerialGetc(hCom);
   SerialPutc(hCom,low);SerialGetc(hCom);
   SerialPutc(hCom,high);SerialGetc(hCom);
   SerialPutc(hCom,0x00);a=SerialGetc(hCom);
   return a;


void Reboot(){
int A,a,b,c,d;
   SerialPutc(hCom,0xAF); A=SerialGetc(hCom);
     SerialPutc(hCom,0x62); a=SerialGetc(hCom);
     SerialPutc(hCom,0x6F); b=SerialGetc(hCom);
     SerialPutc(hCom,0x6F); c=SerialGetc(hCom);
     SerialPutc(hCom,0x74); d=SerialGetc(hCom);
   


               if(a==0x42 && b==0x4f && c==0x4f && d==0x54){SetStatusText("Boot sequence OK");ROBONOVA.bootsequence=1;}
            else{
               sprintf(sts,"[0%x] Boot sequence strange: %x %x %x %x - RESTARTING",A,a,b,c,d);
               SetStatusText(sts);
               Reboot();
            }
      
 }
void Servo_move(int which,int position){
 
     SerialPutc(hCom,0xE6);SerialGetc(hCom);      //move servo
     SerialPutc(hCom,which);SerialGetc(hCom);      //ktore serwo
     SerialPutc(hCom,position);SerialGetc(hCom);   //position
 
}


void MOVEG6A(int a, int b, int c, int d,int e,int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);

SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6B(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,6);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6C(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,12);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom);   //servo IR
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6D(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,18);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}


void MOVE24(int a, int b, int c, int d, int e ,int f, int g, int h, int i, int j , int k, int l,int m, int n,int o, int p, int q, int r, int s,int t, int u, int v, int w, int x){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,24);SerialGetc(hCom);
//Off we go.
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
SerialPutc(hCom,g);SerialGetc(hCom);
SerialPutc(hCom,h);SerialGetc(hCom);
SerialPutc(hCom,i);SerialGetc(hCom);
SerialPutc(hCom,j);SerialGetc(hCom);
SerialPutc(hCom,k);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom);      //servo iR
SerialPutc(hCom,m);SerialGetc(hCom);
SerialPutc(hCom,n);SerialGetc(hCom);
SerialPutc(hCom,o);SerialGetc(hCom);
SerialPutc(hCom,p);SerialGetc(hCom);
SerialPutc(hCom,q);SerialGetc(hCom);
SerialPutc(hCom,r);SerialGetc(hCom);
SerialPutc(hCom,s);SerialGetc(hCom);
SerialPutc(hCom,t);SerialGetc(hCom);
SerialPutc(hCom,u);SerialGetc(hCom);
SerialPutc(hCom,v);SerialGetc(hCom);
SerialPutc(hCom,w);SerialGetc(hCom);
SerialPutc(hCom,x);SerialGetc(hCom);
 
}
void SYNCMOVE(){
int A,a,b,c,d,e;
 
    SerialPutc(hCom,0xEB);SerialGetc(hCom);//sync move
    SerialPutc(hCom,0x06);SerialGetc(hCom);// start from servo no
    SerialPutc(hCom,0x02);SerialGetc(hCom);// how many servoes ?
    SerialPutc(hCom,0x64);SerialGetc(hCom);// servo1 pos
   SerialPutc(hCom,0x64);SerialGetc(hCom);// servox pos
 
   sprintf(sts,"WRONG %x %x %x %x %x",A,b,c,d,e);SetStatusText(sts);
     
}


void Servo_enable(int which){
     SerialPutc(hCom,0xE7);// turn on the servo
     SerialPutc(hCom,which);//ktore serwo
 }

void AD_getdata(int port){
         int b=0;
        SerialPutc(hCom,0xE2);  //Get data from AD
        SerialGetc(hCom);
         SerialPutc(hCom,port);  //Pobieramy wartosc z AD.port
        SerialGetc(hCom);
        SerialPutc(hCom,0x00);  //NULL       
        b=SerialGetc(hCom);
        ROBONOVA.sensor[port].value=b;

}
 
 
void Servo_setspeed(int speed){
SerialPutc(hCom,0xE9);SerialGetc(hCom);//funkcja predkosc serwa
SerialPutc(hCom,speed);SerialGetc(hCom);//predkosc
 }
 


   //Konwertuje wartosc z czujnika na centymetry.
   float IR_centimeter(int wartosc){
        float oblicz=0;
        if(wartosc>0){
        oblicz=(1285/wartosc)*1.1060;
        return oblicz;}else return 0;
   }



int COM_open(){

DCB dcb;
    BOOL fSuccess;
   char *pcCommPort = "COM1";

   hCom = CreateFile( pcCommPort,
                    GENERIC_READ | GENERIC_WRITE,
                    0,    // must be opened with exclusive-access
                    NULL, // no security attributes
                    OPEN_EXISTING, // must use OPEN_EXISTING
                    0,    // not overlapped I/O
                    NULL  // hTemplate must be NULL for comm devices
                    );

   if (hCom == INVALID_HANDLE_VALUE)
   {
       // Handle the error.
       printf ("CreateFile failed with error %d.\n", GetLastError());
       return (1);
   }

   // Build on the current configuration, and skip setting the size
   // of the input and output buffers with SetupComm.

   fSuccess = GetCommState(hCom, &dcb);

   if (!fSuccess)
   {
      // Handle the error.
      printf ("GetCommState failed with error %d.\n", GetLastError());
      return (2);
   }

   // Fill in DCB: 57,600 bps, 8 data bits, no parity, and 1 stop bit.

   dcb.BaudRate = CBR_9600;     // set the baud rate
   dcb.ByteSize = 8;             // data size, xmit, and rcv
   dcb.Parity = NOPARITY;        // no parity bit
   dcb.StopBits = ONESTOPBIT;    // one stop bit

   fSuccess = SetCommState(hCom, &dcb);

   if (!fSuccess)
   {
      // Handle the error.
     MessageBox(0,"ERROR COM","",MB_OK);
      return (3);
   }

 
 
  //  hThread_networking = CreateThread( NULL, 0, THREAD_listenpackets, NULL,  0, &threadId_networking );
    return (0);

   }
int SerialGetc(HANDLE hCom)
{
char rxchar=0;
BOOL bReadRC;
  char z[10];
 static DWORD iBytesRead=NULL;;
 memset(z,0,sizeof(z));

   bReadRC = ReadFile(hCom, &rxchar, 1, &iBytesRead, NULL); 

   if(bReadRC!=TRUE || iBytesRead<1){
      Sleep(1);
      SerialGetc(hCom);
    }else{
 
  sprintf(z,"%i",rxchar);
   return atoi(z);
  }
 
}

void SerialPutc(HANDLE hCom, char txchar)
{
BOOL bWriteRC;
static DWORD iBytesWritten;
bWriteRC = WriteFile(hCom, &txchar, 1, &iBytesWritten,NULL);
if(bWriteRC!=TRUE || iBytesWritten<1>=320 ){
               sprintf(sts,"%x%x = %i ",b,a,c);
               SetStatusText(sts);
               ROBONOVA.servo[wartosc-320].value=c;

            }
         
            /*

To keep you going:
The desired positions are stored in 24 memory locations from &H300.
The current positions are stored in 24 memory locations from &H320
They can both be PEEKed there.

When a move is started a bit is cleared in locations &H4E3, &H4E4, &H4E5.
there is one bit per servo starting at bit 0 in &H4E3.
When the current = the desired, then the corresponding bit is set.
A move is complete when all 3 bytes = &HFF


            */
         //   sprintf(PROCESSOR_status,"Memory red: 0x%x [%x,%x value: %i]",a,SerialGetc(hCom),SerialGetc(hCom),SerialGetc(hCom) );

         break;

    
         case 0xffffffd2:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            b=SerialGetc(hCom);
            
            ROBONOVA.servo[a].value=b;
            sprintf(GL_status,"%i=%i",a,b);


         break;

         case 0xffffffe2:

             a=0;b=0;c=0;d=0;
             a=SerialGetc(hCom);
             b=SerialGetc(hCom);
            
            sprintf(sts,"[0x%x] Data read from AD.%i, value=%i\n",A,a,b);
            SetStatusText(sts);
            ROBONOVA.sensor[a].value=b;
         
         break;

         case 0xffffffe9:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            sprintf(sts,"[0x%x] Servo speed to %x",A,a);
            SetStatusText(sts);

         break;
         case 0xffffffe6:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            b=SerialGetc(hCom);
            sprintf(sts,"[0x%x] Servo: %i to position %i",A,a,b);
            SetStatusText(sts);
         break;
         case 0xffffffe7:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            b=SerialGetc(hCom);
            sprintf(sts,"[0x%x] Servo: %i Enabled. %x",A,a,b);
            SetStatusText(sts);
            ROBONOVA.servo[a].enabled=1;

         break;


   
      
       

      }

      
 

   }
   
return 0;
}
 
Here is my serial code, that drives my RN1 board directly from W32 C++ application.


Code: Select all
         //max odleglosc sensora IR

HANDLE hThread_networking ;
DWORD  threadId_networking ;
HANDLE hCom;
char sts[128];
void Servo_readposition(int servo);   
void Servo_syncmove(int ptp,int firstservo,int lastservo,int value);
void SerialPutc(HANDLE hCom, char txchar);
int SerialGetc(HANDLE hCom);
int IR_getdata();
float IR_centimeter(int wartosc);
void Servo_enable(int which);
void Servo_move(int which,int position);
void Servo_setspeed(int speed);
void AD_getdata(int port);
void Servo_readgroup6A();
void Servo_readgroup6B();
void Servo_readgroup6C();
void Servo_readgroup6D();
int Memory_read(int low,int high);

DWORD WINAPI THREAD_listenpackets(void *parm);
void Servo_wait(int ktore,int wartosc){
   int a=0;
   while(ROBONOVA.servo[ktore].value!=wartosc){
      if(GetTickCount()-tick>10){
         a=Memory_read(32+ktore,0x03);
         ROBONOVA.servo[ktore].value=a;
         tick=GetTickCount();
      }
   }
}

float Voltage(){
int value;
AD_getdata(5);
return (10*ROBONOVA.sensor[5].value)/256;
}
int Memory_read(int low,int high){
   int a;
   SerialPutc(hCom,0xF7);SerialGetc(hCom);
   SerialPutc(hCom,low);SerialGetc(hCom);
   SerialPutc(hCom,high);SerialGetc(hCom);
   SerialPutc(hCom,0x00);a=SerialGetc(hCom);
   return a;


void Reboot(){
int A,a,b,c,d;
   SerialPutc(hCom,0xAF); A=SerialGetc(hCom);
     SerialPutc(hCom,0x62); a=SerialGetc(hCom);
     SerialPutc(hCom,0x6F); b=SerialGetc(hCom);
     SerialPutc(hCom,0x6F); c=SerialGetc(hCom);
     SerialPutc(hCom,0x74); d=SerialGetc(hCom);
   


               if(a==0x42 && b==0x4f && c==0x4f && d==0x54){SetStatusText("Boot sequence OK");ROBONOVA.bootsequence=1;}
            else{
               sprintf(sts,"[0%x] Boot sequence strange: %x %x %x %x - RESTARTING",A,a,b,c,d);
               SetStatusText(sts);
               Reboot();
            }
      
 }
void Servo_move(int which,int position){
 
     SerialPutc(hCom,0xE6);SerialGetc(hCom);      //move servo
     SerialPutc(hCom,which);SerialGetc(hCom);      //ktore serwo
     SerialPutc(hCom,position);SerialGetc(hCom);   //position
 
}


void MOVEG6A(int a, int b, int c, int d,int e,int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);

SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6B(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,6);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6C(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,12);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom);   //servo IR
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6D(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,18);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}


void MOVE24(int a, int b, int c, int d, int e ,int f, int g, int h, int i, int j , int k, int l,int m, int n,int o, int p, int q, int r, int s,int t, int u, int v, int w, int x){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,24);SerialGetc(hCom);
//Off we go.
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
SerialPutc(hCom,g);SerialGetc(hCom);
SerialPutc(hCom,h);SerialGetc(hCom);
SerialPutc(hCom,i);SerialGetc(hCom);
SerialPutc(hCom,j);SerialGetc(hCom);
SerialPutc(hCom,k);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom);      //servo iR
SerialPutc(hCom,m);SerialGetc(hCom);
SerialPutc(hCom,n);SerialGetc(hCom);
SerialPutc(hCom,o);SerialGetc(hCom);
SerialPutc(hCom,p);SerialGetc(hCom);
SerialPutc(hCom,q);SerialGetc(hCom);
SerialPutc(hCom,r);SerialGetc(hCom);
SerialPutc(hCom,s);SerialGetc(hCom);
SerialPutc(hCom,t);SerialGetc(hCom);
SerialPutc(hCom,u);SerialGetc(hCom);
SerialPutc(hCom,v);SerialGetc(hCom);
SerialPutc(hCom,w);SerialGetc(hCom);
SerialPutc(hCom,x);SerialGetc(hCom);
 
}
void SYNCMOVE(){
int A,a,b,c,d,e;
 
    SerialPutc(hCom,0xEB);SerialGetc(hCom);//sync move
    SerialPutc(hCom,0x06);SerialGetc(hCom);// start from servo no
    SerialPutc(hCom,0x02);SerialGetc(hCom);// how many servoes ?
    SerialPutc(hCom,0x64);SerialGetc(hCom);// servo1 pos
   SerialPutc(hCom,0x64);SerialGetc(hCom);// servox pos
 
   sprintf(sts,"WRONG %x %x %x %x %x",A,b,c,d,e);SetStatusText(sts);
     
}


void Servo_enable(int which){
     SerialPutc(hCom,0xE7);// turn on the servo
     SerialPutc(hCom,which);//ktore serwo
 }

void AD_getdata(int port){
         int b=0;
        SerialPutc(hCom,0xE2);  //Get data from AD
        SerialGetc(hCom);
         SerialPutc(hCom,port);  //Pobieramy wartosc z AD.port
        SerialGetc(hCom);
        SerialPutc(hCom,0x00);  //NULL       
        b=SerialGetc(hCom);
        ROBONOVA.sensor[port].value=b;

}
 
 
void Servo_setspeed(int speed){
SerialPutc(hCom,0xE9);SerialGetc(hCom);//funkcja predkosc serwa
SerialPutc(hCom,speed);SerialGetc(hCom);//predkosc
 }
 


   //Konwertuje wartosc z czujnika na centymetry.
   float IR_centimeter(int wartosc){
        float oblicz=0;
        if(wartosc>0){
        oblicz=(1285/wartosc)*1.1060;
        return oblicz;}else return 0;
   }



int COM_open(){

DCB dcb;
    BOOL fSuccess;
   char *pcCommPort = "COM1";

   hCom = CreateFile( pcCommPort,
                    GENERIC_READ | GENERIC_WRITE,
                    0,    // must be opened with exclusive-access
                    NULL, // no security attributes
                    OPEN_EXISTING, // must use OPEN_EXISTING
                    0,    // not overlapped I/O
                    NULL  // hTemplate must be NULL for comm devices
                    );

   if (hCom == INVALID_HANDLE_VALUE)
   {
       // Handle the error.
       printf ("CreateFile failed with error %d.\n", GetLastError());
       return (1);
   }

   // Build on the current configuration, and skip setting the size
   // of the input and output buffers with SetupComm.

   fSuccess = GetCommState(hCom, &dcb);

   if (!fSuccess)
   {
      // Handle the error.
      printf ("GetCommState failed with error %d.\n", GetLastError());
      return (2);
   }

   // Fill in DCB: 57,600 bps, 8 data bits, no parity, and 1 stop bit.

   dcb.BaudRate = CBR_9600;     // set the baud rate
   dcb.ByteSize = 8;             // data size, xmit, and rcv
   dcb.Parity = NOPARITY;        // no parity bit
   dcb.StopBits = ONESTOPBIT;    // one stop bit

   fSuccess = SetCommState(hCom, &dcb);

   if (!fSuccess)
   {
      // Handle the error.
     MessageBox(0,"ERROR COM","",MB_OK);
      return (3);
   }

 
 
  //  hThread_networking = CreateThread( NULL, 0, THREAD_listenpackets, NULL,  0, &threadId_networking );
    return (0);

   }
int SerialGetc(HANDLE hCom)
{
char rxchar=0;
BOOL bReadRC;
  char z[10];
 static DWORD iBytesRead=NULL;;
 memset(z,0,sizeof(z));

   bReadRC = ReadFile(hCom, &rxchar, 1, &iBytesRead, NULL); 

   if(bReadRC!=TRUE || iBytesRead<1){
      Sleep(1);
      SerialGetc(hCom);
    }else{
 
  sprintf(z,"%i",rxchar);
   return atoi(z);
  }
 
}

void SerialPutc(HANDLE hCom, char txchar)
{
BOOL bWriteRC;
static DWORD iBytesWritten;
bWriteRC = WriteFile(hCom, &txchar, 1, &iBytesWritten,NULL);
if(bWriteRC!=TRUE || iBytesWritten<1>=320 ){
               sprintf(sts,"%x%x = %i ",b,a,c);
               SetStatusText(sts);
               ROBONOVA.servo[wartosc-320].value=c;

            }
         
            /*

To keep you going:
The desired positions are stored in 24 memory locations from &H300.
The current positions are stored in 24 memory locations from &H320
They can both be PEEKed there.

When a move is started a bit is cleared in locations &H4E3, &H4E4, &H4E5.
there is one bit per servo starting at bit 0 in &H4E3.
When the current = the desired, then the corresponding bit is set.
A move is complete when all 3 bytes = &HFF


            */
         //   sprintf(PROCESSOR_status,"Memory red: 0x%x [%x,%x value: %i]",a,SerialGetc(hCom),SerialGetc(hCom),SerialGetc(hCom) );

         break;

    
         case 0xffffffd2:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            b=SerialGetc(hCom);
            
            ROBONOVA.servo[a].value=b;
            sprintf(GL_status,"%i=%i",a,b);


         break;

         case 0xffffffe2:

             a=0;b=0;c=0;d=0;
             a=SerialGetc(hCom);
             b=SerialGetc(hCom);
            
            sprintf(sts,"[0x%x] Data read from AD.%i, value=%i\n",A,a,b);
            SetStatusText(sts);
            ROBONOVA.sensor[a].value=b;
         
         break;

         case 0xffffffe9:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            sprintf(sts,"[0x%x] Servo speed to %x",A,a);
            SetStatusText(sts);

         break;
         case 0xffffffe6:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            b=SerialGetc(hCom);
            sprintf(sts,"[0x%x] Servo: %i to position %i",A,a,b);
            SetStatusText(sts);
         break;
         case 0xffffffe7:
            a=0;b=0;c=0;d=0;
            a=SerialGetc(hCom);
            b=SerialGetc(hCom);
            sprintf(sts,"[0x%x] Servo: %i Enabled. %x",A,a,b);
            SetStatusText(sts);
            ROBONOVA.servo[a].enabled=1;

         break;


   
      
       

      }

      
 

   }
   
return 0;
}
 
roboTT offline
Savvy Roboteer
Savvy Roboteer
Posts: 62
Joined: Mon Mar 10, 2008 10:06 am

Post by kushlik » Sun Aug 23, 2009 6:26 am

Post by kushlik
Sun Aug 23, 2009 6:26 am

Hello All,

I thought I'd post a link to my implementation of the Atmel firmware for robonova. Simply compiling and flashing the firmware, will let you command all 16 servos at the same time at 100+ Hz. Also feedback can be requested at 100+ Hz. Other functionality includes the speed setting and reading 5AD channels (for IMU, for example).

http://fling.seas.upenn.edu/~akushley/c ... e.Robonova

Use AVR Studio to compile the Robonova atmel code. The interface (from PC) is only available for Linux via C++ and Matlab.

I have written this up over a year ago, but forgot to post it...
Hello All,

I thought I'd post a link to my implementation of the Atmel firmware for robonova. Simply compiling and flashing the firmware, will let you command all 16 servos at the same time at 100+ Hz. Also feedback can be requested at 100+ Hz. Other functionality includes the speed setting and reading 5AD channels (for IMU, for example).

http://fling.seas.upenn.edu/~akushley/c ... e.Robonova

Use AVR Studio to compile the Robonova atmel code. The interface (from PC) is only available for Linux via C++ and Matlab.

I have written this up over a year ago, but forgot to post it...
kushlik offline
Robot Builder
Robot Builder
Posts: 8
Joined: Mon Jun 02, 2008 10:04 pm

Post by blues » Thu Nov 04, 2010 4:25 pm

Post by blues
Thu Nov 04, 2010 4:25 pm

//max odleglosc sensora IR;

HANDLE hThread_networking;
DWORD threadId_networking;
HANDLE hCom;
char sts[128];
void Servo_readposition(int servo);
void Servo_syncmove(int ptp,int firstservo,int lastservo,int value);
void SerialPutc(HANDLE hCom, char txchar);
int SerialGetc(HANDLE hCom);
int IR_getdata();
float IR_centimeter(int wartosc);
void Servo_enable(int which);
void Servo_move(int which,int position);
void Servo_setspeed(int speed);
void AD_getdata(int port);
void Servo_readgroup6A();
void Servo_readgroup6B();
void Servo_readgroup6C();
void Servo_readgroup6D();
int Memory_read(int low,int high);

DWORD WINAPI THREAD_listenpackets(void *parm);
void Servo_wait(int ktore,int wartosc){


int a=0;
while(ROBONOVA.servo[ktore].value!=wartosc){
if(GetTickCount()-tick>10){
a=Memory_read(32+ktore,0x03);
ROBONOVA.servo[ktore].value=a;
tick=GetTickCount();
}
}
}

float Voltage(){
int value;
AD_getdata(5);
return (10*ROBONOVA.sensor[5].value)/256;
}
int Memory_read(int low,int high){
int a;
SerialPutc(hCom,0xF7);SerialGetc(hCom);
SerialPutc(hCom,low);SerialGetc(hCom);
SerialPutc(hCom,high);SerialGetc(hCom);
SerialPutc(hCom,0x00);a=SerialGetc(hCom);
return a;
}

void Reboot(){
int A,a,b,c,d;
SerialPutc(hCom,0xAF); A=SerialGetc(hCom);
SerialPutc(hCom,0x62); a=SerialGetc(hCom);
SerialPutc(hCom,0x6F); b=SerialGetc(hCom);
SerialPutc(hCom,0x6F); c=SerialGetc(hCom);
SerialPutc(hCom,0x74); d=SerialGetc(hCom);



if(a==0x42 && b==0x4f && c==0x4f && d==0x54){SetStatusText("Boot sequence OK");ROBONOVA.bootsequence=1;}
else{
sprintf(sts,"[0%x] Boot sequence strange: %x %x %x %x - RESTARTING",A,a,b,c,d);
SetStatusText(sts);
Reboot();
}

}
void Servo_move(int which,int position){

SerialPutc(hCom,0xE6);SerialGetc(hCom); //move servo
SerialPutc(hCom,which);SerialGetc(hCom); //ktore serwo
SerialPutc(hCom,position);SerialGetc(hCom); //position

}


void MOVEG6A(int a, int b, int c, int d,int e,int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);

SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6B(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,6);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6C(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,12);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom); //servo IR
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6D(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,18);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}


void MOVE24(int a, int b, int c, int d, int e ,int f, int g, int h, int i, int j , int k, int l,int m, int n,int o, int p, int q, int r, int s,int t, int u, int v, int w, int x){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,24);SerialGetc(hCom);
//Off we go.
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
SerialPutc(hCom,g);SerialGetc(hCom);
SerialPutc(hCom,h);SerialGetc(hCom);
SerialPutc(hCom,i);SerialGetc(hCom);
SerialPutc(hCom,j);SerialGetc(hCom);
SerialPutc(hCom,k);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom); //servo iR
SerialPutc(hCom,m);SerialGetc(hCom);
SerialPutc(hCom,n);SerialGetc(hCom);
SerialPutc(hCom,o);SerialGetc(hCom);
SerialPutc(hCom,p);SerialGetc(hCom);
SerialPutc(hCom,q);SerialGetc(hCom);
SerialPutc(hCom,r);SerialGetc(hCom);
SerialPutc(hCom,s);SerialGetc(hCom);
SerialPutc(hCom,t);SerialGetc(hCom);
SerialPutc(hCom,u);SerialGetc(hCom);
SerialPutc(hCom,v);SerialGetc(hCom);
SerialPutc(hCom,w);SerialGetc(hCom);
SerialPutc(hCom,x);SerialGetc(hCom);

}
void SYNCMOVE(){
int A,a,b,c,d,e;

SerialPutc(hCom,0xEB);SerialGetc(hCom);//sync move
SerialPutc(hCom,0x06);SerialGetc(hCom);// start from servo no
SerialPutc(hCom,0x02);SerialGetc(hCom);// how many servoes ?
SerialPutc(hCom,0x64);SerialGetc(hCom);// servo1 pos
SerialPutc(hCom,0x64);SerialGetc(hCom);// servox pos

sprintf(sts,"WRONG %x %x %x %x %x",A,b,c,d,e);SetStatusText(sts);

}


void Servo_enable(int which){
SerialPutc(hCom,0xE7);// turn on the servo
SerialPutc(hCom,which);//ktore serwo
}

void AD_getdata(int port){
int b=0;
SerialPutc(hCom,0xE2); //Get data from AD
SerialGetc(hCom);
SerialPutc(hCom,port); //Pobieramy wartosc z AD.port
SerialGetc(hCom);
SerialPutc(hCom,0x00); //NULL
b=SerialGetc(hCom);
ROBONOVA.sensor[port].value=b;

}


void Servo_setspeed(int speed){
SerialPutc(hCom,0xE9);SerialGetc(hCom);//funkcja predkosc serwa
SerialPutc(hCom,speed);SerialGetc(hCom);//predkosc
}



//Konwertuje wartosc z czujnika na centymetry.
float IR_centimeter(int wartosc){
float oblicz=0;
if(wartosc>0){
oblicz=(1285/wartosc)*1.1060;
return oblicz;}else return 0;
}



int COM_open(){

DCB dcb;
BOOL fSuccess;
char *pcCommPort = "COM1";

hCom = CreateFile( pcCommPort,
GENERIC_READ | GENERIC_WRITE,
0, // must be opened with exclusive-access
NULL, // no security attributes
OPEN_EXISTING, // must use OPEN_EXISTING
0, // not overlapped I/O
NULL // hTemplate must be NULL for comm devices
);

if (hCom == INVALID_HANDLE_VALUE)
{
// Handle the error.
printf ("CreateFile failed with error %d.\n", GetLastError());
return (1);
}

// Build on the current configuration, and skip setting the size
// of the input and output buffers with SetupComm.

fSuccess = GetCommState(hCom, &dcb);

if (!fSuccess)
{
// Handle the error.
printf ("GetCommState failed with error %d.\n", GetLastError());
return (2);
}

// Fill in DCB: 57,600 bps, 8 data bits, no parity, and 1 stop bit.

dcb.BaudRate = CBR_9600; // set the baud rate
dcb.ByteSize = 8; // data size, xmit, and rcv
dcb.Parity = NOPARITY; // no parity bit
dcb.StopBits = ONESTOPBIT; // one stop bit

fSuccess = SetCommState(hCom, &dcb);

if (!fSuccess)
{
// Handle the error.
MessageBox(0,"ERROR COM","",MB_OK);
return (3);
}



// hThread_networking = CreateThread( NULL, 0, THREAD_listenpackets, NULL, 0, &threadId_networking );
return (0);

}
int SerialGetc(HANDLE hCom)
{
char rxchar=0;
BOOL bReadRC;
char z[10];
static DWORD iBytesRead=NULL;;
memset(z,0,sizeof(z));

bReadRC = ReadFile(hCom, &rxchar, 1, &iBytesRead, NULL);

if(bReadRC!=TRUE || iBytesRead<1){
Sleep(1);
SerialGetc(hCom);
}else{

sprintf(z,"%i",rxchar);
return atoi(z);
}

}

void SerialPutc(HANDLE hCom, char txchar)
{
BOOL bWriteRC;
static DWORD iBytesWritten;
bWriteRC = WriteFile(hCom, &txchar, 1, &iBytesWritten,NULL);
if(bWriteRC!=TRUE || iBytesWritten<1>=320 ){
sprintf(sts,"%x%x = %i ",b,a,c);
SetStatusText(sts);
ROBONOVA.servo[wartosc-320].value=c;

}

/*

To keep you going:
The desired positions are stored in 24 memory locations from &H300.
The current positions are stored in 24 memory locations from &H320
They can both be PEEKed there.

When a move is started a bit is cleared in locations &H4E3, &H4E4, &H4E5.
there is one bit per servo starting at bit 0 in &H4E3.
When the current = the desired, then the corresponding bit is set.
A move is complete when all 3 bytes = &HFF


*/
// sprintf(PROCESSOR_status,"Memory red: 0x%x [%x,%x value: %i]",a,SerialGetc(hCom),SerialGetc(hCom),SerialGetc(hCom) );

break;


case 0xffffffd2:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);

ROBONOVA.servo[a].value=b;
sprintf(GL_status,"%i=%i",a,b);


break;

case 0xffffffe2:

a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);

sprintf(sts,"[0x%x] Data read from AD.%i, value=%i\n",A,a,b);
SetStatusText(sts);
ROBONOVA.sensor[a].value=b;

break;

case 0xffffffe9:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
sprintf(sts,"[0x%x] Servo speed to %x",A,a);
SetStatusText(sts);

break;
case 0xffffffe6:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);
sprintf(sts,"[0x%x] Servo: %i to position %i",A,a,b);
SetStatusText(sts);
break;
case 0xffffffe7:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);
sprintf(sts,"[0x%x] Servo: %i Enabled. %x",A,a,b);
SetStatusText(sts);
ROBONOVA.servo[a].enabled=1;

break;






}




}

return 0;
}


Hi,

I could read that this code is for comunicate with a Robonova with C++
but I could see that are not include the library, could you write wich librarys need you?

Thank you so much.
//max odleglosc sensora IR;

HANDLE hThread_networking;
DWORD threadId_networking;
HANDLE hCom;
char sts[128];
void Servo_readposition(int servo);
void Servo_syncmove(int ptp,int firstservo,int lastservo,int value);
void SerialPutc(HANDLE hCom, char txchar);
int SerialGetc(HANDLE hCom);
int IR_getdata();
float IR_centimeter(int wartosc);
void Servo_enable(int which);
void Servo_move(int which,int position);
void Servo_setspeed(int speed);
void AD_getdata(int port);
void Servo_readgroup6A();
void Servo_readgroup6B();
void Servo_readgroup6C();
void Servo_readgroup6D();
int Memory_read(int low,int high);

DWORD WINAPI THREAD_listenpackets(void *parm);
void Servo_wait(int ktore,int wartosc){


int a=0;
while(ROBONOVA.servo[ktore].value!=wartosc){
if(GetTickCount()-tick>10){
a=Memory_read(32+ktore,0x03);
ROBONOVA.servo[ktore].value=a;
tick=GetTickCount();
}
}
}

float Voltage(){
int value;
AD_getdata(5);
return (10*ROBONOVA.sensor[5].value)/256;
}
int Memory_read(int low,int high){
int a;
SerialPutc(hCom,0xF7);SerialGetc(hCom);
SerialPutc(hCom,low);SerialGetc(hCom);
SerialPutc(hCom,high);SerialGetc(hCom);
SerialPutc(hCom,0x00);a=SerialGetc(hCom);
return a;
}

void Reboot(){
int A,a,b,c,d;
SerialPutc(hCom,0xAF); A=SerialGetc(hCom);
SerialPutc(hCom,0x62); a=SerialGetc(hCom);
SerialPutc(hCom,0x6F); b=SerialGetc(hCom);
SerialPutc(hCom,0x6F); c=SerialGetc(hCom);
SerialPutc(hCom,0x74); d=SerialGetc(hCom);



if(a==0x42 && b==0x4f && c==0x4f && d==0x54){SetStatusText("Boot sequence OK");ROBONOVA.bootsequence=1;}
else{
sprintf(sts,"[0%x] Boot sequence strange: %x %x %x %x - RESTARTING",A,a,b,c,d);
SetStatusText(sts);
Reboot();
}

}
void Servo_move(int which,int position){

SerialPutc(hCom,0xE6);SerialGetc(hCom); //move servo
SerialPutc(hCom,which);SerialGetc(hCom); //ktore serwo
SerialPutc(hCom,position);SerialGetc(hCom); //position

}


void MOVEG6A(int a, int b, int c, int d,int e,int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);

SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6B(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,6);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6C(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,12);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom); //servo IR
SerialPutc(hCom,f);SerialGetc(hCom);
}

void MOVEG6D(int a, int b, int c, int d,int e, int f){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,18);SerialGetc(hCom);
SerialPutc(hCom,6);SerialGetc(hCom);
//
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
}


void MOVE24(int a, int b, int c, int d, int e ,int f, int g, int h, int i, int j , int k, int l,int m, int n,int o, int p, int q, int r, int s,int t, int u, int v, int w, int x){
SerialPutc(hCom,0xEB);SerialGetc(hCom);//SYNC MOVE
SerialPutc(hCom,0);SerialGetc(hCom);
SerialPutc(hCom,24);SerialGetc(hCom);
//Off we go.
SerialPutc(hCom,a);SerialGetc(hCom);
SerialPutc(hCom,b);SerialGetc(hCom);
SerialPutc(hCom,c);SerialGetc(hCom);
SerialPutc(hCom,d);SerialGetc(hCom);
SerialPutc(hCom,e);SerialGetc(hCom);
SerialPutc(hCom,f);SerialGetc(hCom);
SerialPutc(hCom,g);SerialGetc(hCom);
SerialPutc(hCom,h);SerialGetc(hCom);
SerialPutc(hCom,i);SerialGetc(hCom);
SerialPutc(hCom,j);SerialGetc(hCom);
SerialPutc(hCom,k);SerialGetc(hCom);
SerialPutc(hCom,0);SerialGetc(hCom); //servo iR
SerialPutc(hCom,m);SerialGetc(hCom);
SerialPutc(hCom,n);SerialGetc(hCom);
SerialPutc(hCom,o);SerialGetc(hCom);
SerialPutc(hCom,p);SerialGetc(hCom);
SerialPutc(hCom,q);SerialGetc(hCom);
SerialPutc(hCom,r);SerialGetc(hCom);
SerialPutc(hCom,s);SerialGetc(hCom);
SerialPutc(hCom,t);SerialGetc(hCom);
SerialPutc(hCom,u);SerialGetc(hCom);
SerialPutc(hCom,v);SerialGetc(hCom);
SerialPutc(hCom,w);SerialGetc(hCom);
SerialPutc(hCom,x);SerialGetc(hCom);

}
void SYNCMOVE(){
int A,a,b,c,d,e;

SerialPutc(hCom,0xEB);SerialGetc(hCom);//sync move
SerialPutc(hCom,0x06);SerialGetc(hCom);// start from servo no
SerialPutc(hCom,0x02);SerialGetc(hCom);// how many servoes ?
SerialPutc(hCom,0x64);SerialGetc(hCom);// servo1 pos
SerialPutc(hCom,0x64);SerialGetc(hCom);// servox pos

sprintf(sts,"WRONG %x %x %x %x %x",A,b,c,d,e);SetStatusText(sts);

}


void Servo_enable(int which){
SerialPutc(hCom,0xE7);// turn on the servo
SerialPutc(hCom,which);//ktore serwo
}

void AD_getdata(int port){
int b=0;
SerialPutc(hCom,0xE2); //Get data from AD
SerialGetc(hCom);
SerialPutc(hCom,port); //Pobieramy wartosc z AD.port
SerialGetc(hCom);
SerialPutc(hCom,0x00); //NULL
b=SerialGetc(hCom);
ROBONOVA.sensor[port].value=b;

}


void Servo_setspeed(int speed){
SerialPutc(hCom,0xE9);SerialGetc(hCom);//funkcja predkosc serwa
SerialPutc(hCom,speed);SerialGetc(hCom);//predkosc
}



//Konwertuje wartosc z czujnika na centymetry.
float IR_centimeter(int wartosc){
float oblicz=0;
if(wartosc>0){
oblicz=(1285/wartosc)*1.1060;
return oblicz;}else return 0;
}



int COM_open(){

DCB dcb;
BOOL fSuccess;
char *pcCommPort = "COM1";

hCom = CreateFile( pcCommPort,
GENERIC_READ | GENERIC_WRITE,
0, // must be opened with exclusive-access
NULL, // no security attributes
OPEN_EXISTING, // must use OPEN_EXISTING
0, // not overlapped I/O
NULL // hTemplate must be NULL for comm devices
);

if (hCom == INVALID_HANDLE_VALUE)
{
// Handle the error.
printf ("CreateFile failed with error %d.\n", GetLastError());
return (1);
}

// Build on the current configuration, and skip setting the size
// of the input and output buffers with SetupComm.

fSuccess = GetCommState(hCom, &dcb);

if (!fSuccess)
{
// Handle the error.
printf ("GetCommState failed with error %d.\n", GetLastError());
return (2);
}

// Fill in DCB: 57,600 bps, 8 data bits, no parity, and 1 stop bit.

dcb.BaudRate = CBR_9600; // set the baud rate
dcb.ByteSize = 8; // data size, xmit, and rcv
dcb.Parity = NOPARITY; // no parity bit
dcb.StopBits = ONESTOPBIT; // one stop bit

fSuccess = SetCommState(hCom, &dcb);

if (!fSuccess)
{
// Handle the error.
MessageBox(0,"ERROR COM","",MB_OK);
return (3);
}



// hThread_networking = CreateThread( NULL, 0, THREAD_listenpackets, NULL, 0, &threadId_networking );
return (0);

}
int SerialGetc(HANDLE hCom)
{
char rxchar=0;
BOOL bReadRC;
char z[10];
static DWORD iBytesRead=NULL;;
memset(z,0,sizeof(z));

bReadRC = ReadFile(hCom, &rxchar, 1, &iBytesRead, NULL);

if(bReadRC!=TRUE || iBytesRead<1){
Sleep(1);
SerialGetc(hCom);
}else{

sprintf(z,"%i",rxchar);
return atoi(z);
}

}

void SerialPutc(HANDLE hCom, char txchar)
{
BOOL bWriteRC;
static DWORD iBytesWritten;
bWriteRC = WriteFile(hCom, &txchar, 1, &iBytesWritten,NULL);
if(bWriteRC!=TRUE || iBytesWritten<1>=320 ){
sprintf(sts,"%x%x = %i ",b,a,c);
SetStatusText(sts);
ROBONOVA.servo[wartosc-320].value=c;

}

/*

To keep you going:
The desired positions are stored in 24 memory locations from &H300.
The current positions are stored in 24 memory locations from &H320
They can both be PEEKed there.

When a move is started a bit is cleared in locations &H4E3, &H4E4, &H4E5.
there is one bit per servo starting at bit 0 in &H4E3.
When the current = the desired, then the corresponding bit is set.
A move is complete when all 3 bytes = &HFF


*/
// sprintf(PROCESSOR_status,"Memory red: 0x%x [%x,%x value: %i]",a,SerialGetc(hCom),SerialGetc(hCom),SerialGetc(hCom) );

break;


case 0xffffffd2:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);

ROBONOVA.servo[a].value=b;
sprintf(GL_status,"%i=%i",a,b);


break;

case 0xffffffe2:

a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);

sprintf(sts,"[0x%x] Data read from AD.%i, value=%i\n",A,a,b);
SetStatusText(sts);
ROBONOVA.sensor[a].value=b;

break;

case 0xffffffe9:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
sprintf(sts,"[0x%x] Servo speed to %x",A,a);
SetStatusText(sts);

break;
case 0xffffffe6:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);
sprintf(sts,"[0x%x] Servo: %i to position %i",A,a,b);
SetStatusText(sts);
break;
case 0xffffffe7:
a=0;b=0;c=0;d=0;
a=SerialGetc(hCom);
b=SerialGetc(hCom);
sprintf(sts,"[0x%x] Servo: %i Enabled. %x",A,a,b);
SetStatusText(sts);
ROBONOVA.servo[a].enabled=1;

break;






}




}

return 0;
}


Hi,

I could read that this code is for comunicate with a Robonova with C++
but I could see that are not include the library, could you write wich librarys need you?

Thank you so much.
blues offline
Newbie
Newbie
Posts: 2
Joined: Thu Nov 04, 2010 4:21 pm

Post by pejus » Wed Dec 08, 2010 8:21 am

Post by pejus
Wed Dec 08, 2010 8:21 am

Did u get any response? I wrote pm to roboTT but he seems to not coming here anymore :(
Did u get any response? I wrote pm to roboTT but he seems to not coming here anymore :(
pejus offline
Newbie
Newbie
Posts: 3
Joined: Tue Nov 09, 2010 12:29 am

Post by blues » Wed Dec 08, 2010 3:37 pm

Post by blues
Wed Dec 08, 2010 3:37 pm

Ok dont worry, anyway thank you for the help. :)
Ok dont worry, anyway thank you for the help. :)
blues offline
Newbie
Newbie
Posts: 2
Joined: Thu Nov 04, 2010 4:21 pm

Next
Next
16 postsPage 1 of 21, 2
16 postsPage 1 of 21, 2