Software modifications and upgrades of Hexy


#21

Ad servotorComm.py - yes, I commented that section because in Windows it was just slowing connection process. But probably you need this under different OS. Sorry for that.
The code was not tested on any other OS or PC than mine (Win XP 32bit, Python 2.7.3). I don’t guarantee that it will work flawlessly across all platforms, but generally it should. That’s why I published the classes piece by piece, so people can try to implement what they want and easier debug the code.
About the error when you force-kill the application, I had something similar before. It was caused by matplotlib/mplot3d plot. I had to google a bit and I found how to correctly kill the app:

class App:
    ...
    def quitApp(self):
        self.con.killAll()
        plt.close('all')
        root.quit()
        root.destroy()

It might be that this in not OK for Ubuntu. I suggest that you make a test application using matplotlib/mplot3d and try to figure out what’'s wrong.


#22

Michal, you said you are working on some other stuff for hexy. Could it be by any chance we will see a fully autonomous hexy? :slight_smile:


#23

Yes, this is one of the projects. But the second one is something completely different and it’s going to be HUGE :sunglasses:


#24

Michal, thanks for the tips. I’ll have a look around that code to see what’s causing the hang in Ubuntu.

TooManySecrets, what do you envisage Hexy doing? Difficulty with autonomous activity is that this is on Hexy’s microcontroller only, so it’s all in C++. Plus on power up he’ll only do one thing until you reflash the microcontroller. NB I conceed that there’s already threads about this topic on the forum, so here probably isn’t the place to discuss.


#25

I want to see the hexy get all its commands from a pc running python and move about avoiding obstecles. I know this is not a place to discuss this but i just wanted to ask Michal if that was one of his projects


#26

Hexy is dancing (now for real!)

http://youtu.be/sU5ZAuzX7nQ

Hi all, the last two weeks I was quite busy with PoCoMo development (about 1500 new lines of code) so today I can present you this new functionality which allows you:

  1. Make Hexy dance in the rhythm of played music
  2. Create your own dance moves in easy-to-use GUI
  3. Create sequences of these moves (playlists)
  4. Test the moves on virtual model of Hexy - safest and easiest way of debugging
  5. Save/Load/Import/Export for sharing

If implemented in standard PoCoMo distribution (after testing and debugging period, of course) I predict following applications:

  1. You make your own Hexy look alive
  2. Cool presentation on fairs - kids will love it -> more people will buy Hexy
  3. As a school projects, you can do dance battles with two Hexys - to show how skilfully you can program yours.

A bit of theory:
Each dance move (e.g. Hip swing) consists of several steps. A step is an atomic element describing static position/orientation of Inverse-kinematic-controlled Hexy. Plus in each steps you can say that the robot should walk (requires always 2 steps) or manually modify positions of up to 6 servos. Each dance move can have unlimited number of steps, but practically you need about 2-8. From the database of your dance moves you can create custom lists/sequences of dance moves and specify how many times should each move be repeated.

Dance control GUI:

  • The top part shows control buttons to start/stop the dance and currently performed dance move. On the right there are indicators of synchronization with played music. Tempo indicator blinks in the detected music tempo (BPM = beats per minute). Beat indicator blinks when beat appears in music. And sync indicator blinks when software decides to synchronize and update tempo based on detected beats. If no music is played you can generate your own tempo.

  • The middle part shows on the right hand side the database of all your dance moves. You can do several standard operations with each move, including import and export. The database is saved automatically after adding or editing action. The left half shows sequence list built from the dance moves. The sequence list stores only ID of the move, which serves as unique identifier (hence moves can have the same name). Moreover, it stores how many times you want to repeat each move. The sequences can be again saved/loaded etc.

  • The bottom part is visible only if you create or edit a dance move. Here you define steps of one dance move. Each step shows position and orientation of the robot + walk action or manual settings of up to 6 servos. When creating or editing a step a dialog window opens (see one figure below) where you can pick from available options.
    Moreover, for each dance move you define Move types - how the steps should be executed based on the tempo. Single - 1 step per beat, Double - twice that fast, Pulse - two steps fast after each other.
    Also you can limit for what kind of music can this dance move be used (Fast, Slow, No beat). No beat means that no beat in music was detected for some time, but the music is playing (typically beginning or intermezzo). During “no beat” action the robot slowly moves between first two steps.


Dialog window for adding or editing of one step

The code

Important: I strongly recommend to use only 4 AA NiMH batteries while performing dance movements. I also DO NOT recommend to use this with plastic-gear servos. The dancing puts quite high stress on the servos. Although I’m using metal-gear servos, I managed to literally BURN two servos while using 5 NiMH batteries for dancing. However, I found that under IK control 4 batteries are enough which is visible on the ease of Hexy getting up (also the demo video was done with only 4 batteries).

And here is the full SW with all modifications so far PoCoMo 1.2 - Michal. I wanted to put the code itself here, but it exceeds the limit of one post.

Unfortunately for full functionality you need to install PyAudio which is cross-platform audio I/O library and required for capturing the music. You can use the code without it, but just with internally generated tempo without any synchronization with played music.

For detecting any sound, you must set input of your audio card to Stereo Mix or other source of sound (probably OS dependent). Successful sound detection is indicated by sound level indicator in the GUI.


#27

Haha Michal this is so funny, nice one on making Hexy even more cute.


#28

That’s the reason why I did this :slight_smile:

I added some pictures and description and also I updated several posts describing binary communication, inverse-kinematic control and virtual model of Hexy. These modifications were necessary to get the thing running smoothly.

EDIT: I just published the full PoCoMo 1.2 - Michal in the previous post. Please read the warnings and recommendations before downloading. I’m not responsible for any damage the software can cause.


#29

Michal, epic. Just epic. Excellent work, as always. You sure you don’t fancy getting a coffee/ beer bought for you?


#30

Thanks! I’m glad that you like it. No beer necessary, just give ‘like’ to the video :smiley:


#31

Optimized Servotor32 for faster servo position update

The impact of the below listed changes to current PoCoMo-controlled Hexy is minimal. You will most probably appreciate this information if you are trying to transfer more control into Servotor32 and you are trying to squeeze some CPU cycles.

I found that when I’m communicating with Servotor32 with my binary protocol, the serial link buffer can get overloaded (and Servotor32 halted) if I send more than about 20 packets per second (20 x 19 servos). 20 FPS is definitely sufficient speed because of mechanical servo speed limit (they cannot move that fast). I personally use 10 FPS. However, I found that the buffer overload is caused by inefficient way of updating the servo position registers one-by-one. This is due to the reason that the original (ASCII) communication sends position update for each servo separately, while my binary communication send 19 servo positions at once.
So before the Servotor32 code looked something like:

if (binary) {
	  switch(inChar){
		case '\r':
		case '\n':
			binary = false;
                        changeServo(5,inAllPos[0]);
                        changeServo(6,inAllPos[1]);
                        changeServo(7,inAllPos[2]);
                        changeServo(9,inAllPos[3]);
                        changeServo(10,inAllPos[4]);
                        changeServo(11,inAllPos[5]);
                        changeServo(13,inAllPos[6]);
                        changeServo(14,inAllPos[7]);
                        changeServo(15,inAllPos[8]);
                        changeServo(16,inAllPos[9]);
                        changeServo(17,inAllPos[10]);
                        changeServo(18,inAllPos[11]);
                        changeServo(20,inAllPos[12]);
                        changeServo(21,inAllPos[13]);
                        changeServo(22,inAllPos[14]);
                        changeServo(24,inAllPos[15]);
                        changeServo(25,inAllPos[16]);
                        changeServo(26,inAllPos[17]);
                        changeServo(31,inAllPos[18]);
		  break;
		default:
                  inAllPos[numCount] = byte(inChar)*10;
                  numCount++;
		  break;
	   }
        }
	else {
	  switch(inChar){
         ...

which called changeServo() function 19 times. changeServo() calls update_registers_fast() which, for a given servo, removes the servo from a sorted list and adds it back based on the new servo position. And then updates shift registers. 19 times for 19 servos.

Hence I made a new function update_all_registers_fast() which build this sorted list for all servos only once and then updates the shift registers:

void Servotor32::update_all_registers_fast(){
  while(update_reg_flag == 0){ // wait for the servos to stop pulsing before updating the timing arrays
    delayMicroseconds(10);
  }
  // ----- delete all ------
  for(byte group=0; group<GROUPS; group++){
    servos_active_in_group[group] = 0;
    active_servos_hex[group] = 0;
    for(byte i=0; i<SERVOS_PER_GROUP; i++){
      servos_sorted[group][i] = -1;
    }
  }
  
  // ----- add all active servos ------
  for(byte i=0; i<SERVOS; i++){
    if (servo_positions[i] != -1) {
      servos_sorted[i/SERVOS_PER_GROUP][servos_active_in_group[i/SERVOS_PER_GROUP]] = i; //index of active servo
      servos_active_in_group[i/SERVOS_PER_GROUP] += 1;
      active_servos_hex[i/SERVOS_PER_GROUP] |= pin_2_num[i%SERVOS_PER_GROUP];
    }
  }
  
  // ----- bubble sort servos_sorted ------
  // sort the array by servos position, the ones with pos = -1 will stay at the end
  boolean sorted = false;
  byte j = 0;
  short temp = 0;
  for(byte group=0; group<GROUPS; group++){   // for each group separately 
     sorted = false;
     j = 0;
     while (!sorted){   // continue sorting the array until is sorted. 
       sorted = true;  // expecting the the array sorted
       for (byte i=0; i<servos_active_in_group[group]-1-j; i++){   // go through the active servo list (they are at the front)
         if (servo_positions[servos_sorted[group][i]] > servo_positions[servos_sorted[group][i+1]]){  //if wrong order of two consecutive, swap them
           temp = servos_sorted[group][i];
           servos_sorted[group][i] = servos_sorted[group][i+1];
           servos_sorted[group][i+1] = temp;
           sorted = false;   // no, it was not sorted yet
         }
       }
       j++;
     }
  }

  // ----- create timing idicies from servo/group data ------- (no change from here on)
  
  // clear the timing arrays for fresh start
  for(uint8_t i=0; i<MAX_TIMINGS; i++){
    servo_timings[i] = 0;
    shift_output[i] = 0xFF;
    shift_latch[i] = 0xFF;
  }

  uint8_t counter_index=0;
  uint8_t current_timing=0;
  uint8_t current_shift_output=0; 
  
  for(byte group=0; group<GROUPS; group++){ //go through each group
    if(servos_active_in_group[group] > 0){ // skip it if the group is active, otherwise:
      servo_timings[counter_index] = group_offsets[group];
      shift_output[counter_index] = active_servos_hex[group];
      shift_latch[counter_index] = (1<<group_latches[group]);
      counter_index +=1;
      
      
      //create additional timings
      for(byte i=0; i<servos_active_in_group[group]; i++){ //create the timings for each servo after that, using the previous output
        if(servo_positions[servos_sorted[group][i]] == servo_positions[servos_sorted[group][i-1]]){ // if this servo's time is the same as the last's
          if(i != 0){
            counter_index -= 1; //reverse the index count-up
          }
          else{
            current_shift_output = shift_output[counter_index-1];
            servo_timings[counter_index] = servo_positions[servos_sorted[group][i]]+ group_offsets[group];
            shift_latch[counter_index] = (1<<group_latches[group]);
          }
        }
        else{
          current_shift_output = shift_output[counter_index-1];
          servo_timings[counter_index] = servo_positions[servos_sorted[group][i]]+ group_offsets[group];
          shift_latch[counter_index] = (1<<group_latches[group]);
        }
        
        //subtract the current servo from the shift register output
        current_shift_output &= ~pin_2_num[servos_sorted[group][i]-group*SERVOS_PER_GROUP]; 
        shift_output[counter_index] = current_shift_output;
        counter_index +=1;
      }
    }      
  }
  
}

And the processChar() function changed to:

// short inAllPos[19];   - removed
uint8_t servosInPacket[] = {5,6,7, 9,10,11, 13,14,15, 16,17,18, 20,21,22, 24,25,26, 31};

void Servotor32::processChar(char inChar){
	if (binary) {
	  switch(inChar){
		case '\r':
		case '\n':
			binary = false;
                        update_all_registers_fast();
		  break;
		default:
                  inPos =  byte(inChar);
                  if ((inPos >= 50) && (inPos <= 250)){
                    servo_positions[servosInPacket[numCount]] = inPos;
                  }
                  if (inPos == 255){
                    servo_positions[servosInPacket[numCount]] = -1;
                  }
                  numCount++;
		  break;
	   }
        }
	else {
	  switch(inChar){
      ...

The original function update_registers_fast() stays in the code. My function update_all_registers_fast() is called only when binary packet is received. This way backward compatibility is preserved.
Now, updating all 19 servos is about 10x faster (more efficient) than it was. The impact of these changes is that Servotor32 does not get halted when you send binary packets at high rate.

As I mentioned at the beginning, this function might be also helpful to you if you are working on autonomous Hexy and therefore you want to update more servos at once as well (with or without serial communication).


#32

I want to start by saying “great job” to Michal for all the work that he has done here.

Next, I would like to say to all listening that I believe it important to preserve the original serial command mechanisms because it provides an easy way to test things by hand and it will maintain backwards compatibility. Also, it should not cause any problems switching between them both the way Michal is currently doing.

That said, I believe that the switch to binary mode should not be triggered using the ‘$’ character because it would be too easy to input this by accident during hand testing. That is because ‘$’ is next to ‘#’ on most keyboards. Accidentally doing this by hand could be disastrous.

My first thought was to move this to ‘&’ because its keyboard is reasonably far away to avoid accidents. However, I have come up with an even better idea, use -4 (0xFB); this will be impossible to type by accident on most keyboards.


#33

Hi guys I have a moving problem. The problem is when I make my hexy move forward or back ward hexy turns a little to the left and after moving forward a few times he’s turn like 20 degrees! I’ve seen michals PoMoCo kinematics control and It moves straight. I think my move problem is hexy is moving to fast. How do I import your kinematics control into the normal PoMoCo?
I have your PoMoCo 1.2 and the kinematics control is NOT working at all. :confused: If you don’t know how to import can you tell me how to fix your PoMoCo? I’m not him. :arrow_right: :ugeek:

Thanks
Walker


#34

[quote=“michal”]Optimized Servotor32 for faster servo position update

The impact of the below listed changes to current PoCoMo-controlled Hexy is minimal. You will most probably appreciate this information if you are trying to transfer more control into Servotor32 and you are trying to squeeze some CPU cycles.
[/quote]

First of all great work Michal, PoMoCo 2.1 is awesome.

I notice that since the release of 2.1 you have made a lot of changes you have mentioned in this thread. Is there any chance that you will be releasing a new version with all these improvements?

Also, I would like to use your version of PoMoCo with a PS3 controller. I will be trying to adapt the code you have to work for both joysticks. Let me know if you are interested in this.

Cheers,
Andrew.


#35

Anyone have a idea on how would i bind the gait selection to a joystick. Im also trying to find a way to get pomoco to change to the Kinematic control screen as soon as it starts up so i don’t need to physically use a mouse to select that tab.

Btw. Michal i cant seam to get your pomoco working. i get this

['BTHENUM\\{00001101-0000-1000-8000-00805F9B34FB}_LOCALMFG&000F\\8&3470B7BB&0&0015FFF41115_C00000002', 'Standard Serial over Bluetooth link (COM4)', 'COM3', 'COM4', 'Standard Serial over Bluetooth link (COM3)', 'BTHENUM\\{00001101-0000-1000-8000-00805F9B34FB}_LOCALMFG&0000\\8&3470B7BB&0&000000000000_00000004'] Attempting to connect to Servotor Connection to Servotor failed. No robot movement will occur. Initializing servos. Servos initialized. Killing all servos. Connect Successful! Connected on port: COM3

The gui opens just fine but no servo movement.


#36

Hello everyone!

We’ll I just ordered, received and installed the new metal gear motor replacements offered by the Arcbotics store!

When installing them, I found that I had to use he new white horns provided as well as the smaller gear mount screw (the new horns have a smaller hole, the new screw was simply too small for the old mount horns.) These motors are very smooth and do not slide in the gear mount like the original motors. They look pretty solid and I expect they will last much longer!

I now have a extra white wire for each motor which believe tells me its current analog position, this could be very nice for more fluid movement transitions from one move to another. Once I figure out a good way to implement it. :slight_smile: My guess is that each motor group on the the controller has a extra 3 pin row, I’m guessing that these could be set within the controller to be analog and they could be then used to get the position of each motor, this in turn would use this value as an offset for moment calculations. Has anyone looked into this already, any idea if my assumptions are correct?

I just wanted to check before I start digging, to save me any time/frustration before I go down this path… I’d love to hear from anyone else who has started using these.

Thanks!
:slight_smile:


#37

Finally got my Hexy working with a TV stick so no need to have my PC in the loop anymore

youtu.be/fehculO_qo4


#38

That’s awesome! How difficult did you find the process of installing PicUntu? Can’t wait till I have some time to try the same :slight_smile:


#39

Thanks, I’m really glad I finally got it working. Its taken a lot longer than I expected.

I spent a couple month stuffing around with different kernels and missing drivers, and eventually compiling kernels. My Linux experience was minimal before this project so took me a while to get my head around the problem. Now that I’ve got it all nutted out its relatively easy: install drivers, flash the kernel, flash the base filesystem, then run through the process of configuring Linux to do what you want.

While its working at the moment I’m missing a few things like auto running PoMoCo on boot and having a nice way to do a clean shutdown. I’m also looking into an easy way to stream video from an onboard camera


#40

Try MJPEG-Streamer:
sourceforge.net/projects/mjpg-streamer/