Trying a "State Machine" for Servo Control

Joe Paul

Joe Paul

Supporting Member
Hi Everyone,

So I was given part of this code, but I don't know what I'm doing incorrectly, but the state machine not working:


Code: 
#define numChannels 2
#include <VarSpeedServo.h> 
byte channelValues[numChannels];
uint8_t readstate = 0;
bool channelsOk = false;


VarSpeedServo servo1;
VarSpeedServo servo2;


void setup() {
    Serial.begin(115200);
    
    servo1.attach(2);
    servo2.attach(3);
   
      servo1.write(95,75);  
      servo2.write(95,75);
     
}


void loop() {
   static uint8_t iChn;
   switch (readstate) {
      case 0: // waiting for a '~'
        if( Serial.read() != '~') break;
        readstate = 1;
        // falls through

     case 1:  // Waiting for a '!'
       if( Serial.read() != '!' ) {
           readstate = 0;
           break;
        }
        iChn = 0;
        readstate = 2;
        channelsOk = false;  // don't update the servos while we are updating their positions
        // falls through

     case 2:  // reading the channelVaues
     byte channelValues[numChannels];

      for(byte iChn = 0; iChn < numChannels; iChn++)
        channelValues[iChn] = Serial.read(); //read channel value from serial data

        if( ++iChn < numChannels ) break;
        iChn = 0;
        readstate = 0;         // reset the read state for the next command stream.  
        channelsOk = true; // ok, now we can move the servos

         servo1.write(map(channelValues[0], 0,255, 84, 120), 75); //Schroeder 90-110
         servo2.write(map(channelValues[1], 0,255, 90, 110), 75); //Patty     90-110
     
   }

    
}


So, any ideas or suggestions appreciated.

Tried to test with the serial monitor, data is not getting through.

Thanks!

Take care, Joe.
 
Last edited:
Here, this is Richie's sketch with the Var speedservo lib added .

effect values count for precision.

Check your pin assignment for the 2 servos also . is 2 and 3 correct ?


Code: 
//#include <Servo.h>
#include <VarSpeedServo.h> 
#define numChannels 8

byte channelValues[numChannels];
Servo servo[numChannels];

void setup() {
  //start serial port
  Serial.begin(115200);
  
  //init servo objects
 // servo[0].attach(10);
  servo[1].attach(2);
  servo[2].attach(3);
  //servo[3].attach(5);
 // servo[4].attach(6);
 // servo[5].attach(9);
 // servo[6].attach(7);
 // servo[7].attach(8);
}

void loop() {  
  for(;;){
    //check for 2 consecutive '>' chars
    while(!Serial.available());
    if(Serial.read() != 0x3E) {continue;}
    
    while(!Serial.available());
    if(Serial.read() != 0x3E) {continue;}
        
    //now read the next bytes (qty = numChannels) into the channelValues array
    for(byte i=0; i<numChannels; i++){
      while(!Serial.available());
      channelValues[i] = Serial.read();
    }

    updateServoPositions();//this function updates the servos
  }
}

void updateServoPositions(){
  for(byte i=0; i<numChannels; i++){
    //servo[i].write(map(channelValues[i], 0, 255, 0, 180));
     servo[i].write(map(channelValues[i], 0, 255, 84, 120), 75); //Schroeder 90-110
    /*uncomment this to print test values to IDE serial monitor
    Serial.print("controller output ");
    Serial.print(i);
    Serial.print(" value = ");
    Serial.print(channelValues[i]);        
    Serial.print("/255 or ");
    Serial.print(map(channelValues[i], 0, 255, 0, 180));
    Serial.println("/180 degrees");
    */
  }
}
 
Last edited:
angus40 said:
Richie Northcott's code in post 11 of your other servo thread pretty well puts this to bed !
Click to expand...

Thanks!!

I am using that code above. (moose video) however, I get the occasional hiccup that might be resolved with a "state machine," so serial can still be received while servos are being driven. So I tried to work out the problem but I'm stumped.
 
is this still a factor --> Also I discovered that we needed "==" instead of "!=" in the line.

This has worked after several restarts; no mis mapping: ?
 
angus40 said:
is this still a factor --> Also I discovered that we needed "==" instead of "!=" in the line.

This has worked after several restarts; no mis mapping: ?
Click to expand...

Thanks, Richie's code is fantastic and works well, but for whatever reason, I get the occasional hiccup which is obvious when the moose's mouth doesn't move when it should, for me. So the "state machine" is supposed to fix that. There is something I am missing, or perhaps I should just settle. I have the same issue no matter the board (Uno or ESP32) Win 11 machine, or if it's the serial or the e131. I found a couple problems in the code I posted first post above, which I didn't write completely myself, and correct those typos. Just to clarify, Richie's code is wonderful. But just me, I get that momentary drop out or hiccup. The serial stops to my board but Vixen is still sending it smoothly enough. Perhaps my cheap PCs.

Take care, Joe.
 
One other thing, Today I had to reinstall Vixen 2 times and I couldn't update my COM ports in 2 profiles. I had to go into the device manager and uninstall those ports. With nothing connected to the PC and no IDE open, Vixen had a couple ports "in use" and Vixen would then be unresponsive if I tried to change them, had to shut it down in task manager. Only solution was to delete those ports. If someone knows a work around, I'd be happy to learn it. I spent hours this morning. There has to be a reason for the drop outs, probably in my code.
 
Joe Paul said:
One other thing, Today I had to reinstall Vixen 2 times and I couldn't update my COM ports in 2 profiles. I had to go into the device manager and uninstall those ports. With nothing connected to the PC and no IDE open, Vixen had a couple ports "in use" and Vixen would then be unresponsive if I tried to change them, had to shut it down in task manager. Only solution was to delete those ports. If someone knows a work around, I'd be happy to learn it. I spent hours this morning. There has to be a reason for the drop outs, probably in my code.
Click to expand...

That would usually mean that something was using the COM port and didn't release it. A reboot should take care of it.
 
ctmal said:
That would usually mean that something was using the COM port and didn't release it. A reboot should take care of it.
Click to expand...

Thanks for the reply! Several, many reboots didn't help. In the past it did, but today. nothings but problems.
 
i looks to me that Ritchie's code is blocking in the for loop at the while statements. If really depends how the rest of the code is set up but this could mean that if you are not receiving any data at a given time that you aren't sending any information out to the servos.

Are the hiccups you mention happening near a time when there's a stop in data coming from Vixen?
 
The state machine code: The first major issue I see is that there aren't any checks for serial available. This a big issue because the device you're running will be much faster than data is sent to the serial port.

So, for example. Lets say you receive a ~ to get from case 0 to case 1. The case 1 code is immediately run and there's probably no chance your serial port has received the next byte. So, case 1 will evaluate to false which will automatically set it back to case 0, and so on.

There's multiple ways to handle this depending on how you want the flow to work. For example, you can wait for the next byte as Ritchie's code does or you can only run the code in each case if there is serial available. It really depends on the flow of your program and how you're handing the output to the servos.
 
ctmal said:
The state machine code: The first major issue I see is that there aren't any checks for serial available. This a big issue because the device you're running will be much faster than data is sent to the serial port.

So, for example. Lets say you receive a ~ to get from case 0 to case 1. The case 1 code is immediately run and there's probably no chance your serial port has received the next byte. So, case 1 will evaluate to false which will automatically set it back to case 0, and so on.

There's multiple ways to handle this depending on how you want the flow to work. For example, you can wait for the next byte as Ritchie's code does or you can only run the code in each case if there is serial available. It really depends on the flow of your program and how you're handing the output to the servos.
Click to expand...

Many thanks, Chris, for the info!!!!

I only want to prevent the lag or drop out that was evident in the moose video. When I used the ESP32 with the Bluetooth and the physical USB connection to the board, I could see that Vixen was sending serial but my board would drop out for split second.
So with the code reduced to only one channel, header or not, I see the drop out. When I reduce the update interval, I see fewer hiccups. So, if you could point me to some example code, I'll try it all.

Does the header need 2 characters? I understand Richie's code and would like to work with that; that's what you see in the first Moose video.

Take care, Joe.
 
Last edited:
Joe Paul said:
Many thanks, Chris, for the info!!!!

I only want to prevent the lag or drop out that was evident in the moose video. When I used the ESP32 with the Bluetooth and the physical USB connection to the board, I could see that Vixen was sending serial but my board would drop out for split second.
So with the code reduced to only one channel, header or not, I see the drop out. When I reduce the update interval, I see fewer hiccups. So, if you could point me to some example code, I'll try it all.

Does the header need 2 characters? I understand Richie's code and would like to work with that; that's what you see in the first Moose video.

Take care, Joe.
Click to expand...

Sorry, I don't know of any sample code for this.
i'm not sure what you mean by update interval. Is this something in your code or is it the rate Vixen is sending data? I did all my servo stuff at 50 ms. That is plenty fast. Here is an example: https://www.youtube.com/watch?v=4cSCiq4FqOI&t=1s go to 40s for a zoomed in look.

It looks to me that its really working well except in that one issue. So, I would think the issue might be the data being sent...i.e., it's either coming in too fast or, and this would just be an amazing coincidence, somehow in the data it inadvertently is sending the header sequence. If you edit the sequence in that spot does the problem go away?

As for the header, yes, you need multiple characters. Keep in mind that the header is only trying to define when the packet starts. If you use only 1 character, every time you send that character from Vixen it can mess things up. Even with 2 characters it is still possible to accidently send that code(which I think might be happening in this case).
 
So, in your case, if Vixen is sending 126 (~) followed by 33 (!) in the area of the issue, I'd bet that is the problem.
 
ctmal said:
So, in your case, if Vixen is sending 126 (~) followed by 33 (!) in the area of the issue, I'd bet that is the problem.
Click to expand...

Hi Chris, many thanks for the reply! Great video!!!! Your skeleton is perfection!! Nice syncing!

So the update interval is what defaults to 50. I'll get more hiccups at 50. 23 works best for me. So my header is "~" and "!" -- I'll try different characters and add a 3rd. The hiccups are random, and I have tried to alter the sequencing, but it doesn't seem to matter.

I gather you're not using an Arduino.

Take care, Joe.
 
ctmal said:
Thank you.
Here is what I'm using:
https://doityourselfchristmas.com/forums/showthread.php?11737-Renard-based-Servo-controller

and for the voice sequencing:
https://doityourselfchristmas.com/forums/showthread.php?17329-Jaw-sequencing-an-easier-way!

If the hiccups are random then I was wrong. I doubt its an issue with the header.

Can you post the full code? It might give everyone a better idea how your doing everything.
Click to expand...

Wonderful work, Chris!!! You've got great programming skills, too!

Code: 
#include <ESP32Servo.h>
#include "BluetoothSerial.h"

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif*

BluetoothSerial SerialBT;
Servo  myservo1;

void setup() {


  Serial.begin(115200);
  SerialBT.begin("ESP32BTServo");

  myservo1.attach(2);

  myservo1.write(95);
 
}

void loop() {
  while (!SerialBT.available());

  for ( ; ; )
  {
    if (SerialBT.read() == '~') {
      break;
    }
  }

  while (!SerialBT.available());
  for ( ; ; )
  {
    if (SerialBT.read() == '!') {
      break;
    }
  }


  byte channelValues;

  {
    while (!SerialBT.available());
    channelValues = SerialBT.read();
   
        if (channelValues <= 0 && channelValues >= 255) {  
          channelValues = 0;}
         
  }

  myservo1.write(map(channelValues, 0, 255, 82, 130)); //moose

}

So the ESP32 Bluetooth doesn't make a difference. Doing straight USB serial on either the Arduino or the ESP32 produces the same behavior. Same problem with 8 or 10 channels, obviously the code is different. I reduced it to the one channel for clarity for myself (old guy).

I am considering the "Jawduino" approach with the little line level board (already bought them). I did Scary Terry's circuit a few years ago where you embed a tone in the track in Audacity. I would remove the vocal and only play that into the board with a mono version on the other. But your idea with the recording of your own voice, I can make the softer parts of the vocals louder for the line-level board.

After a good week, several hours a day, on these hiccups, I'm ready to move on.

So, with the code above, I don't need or see any difference with or without the headers. Three characters seemed to create more hiccups, but not sure.

Thanks and take care, Joe.
 
Last edited:
I'll look over it more tomorrow or Friday but the first thing I notice is that you do a lot of waiting for data coming in. This means that the loop is pause and possibly information is not going out to the servos.
I could be wrong on that but I'll have to look at the ESP32Servo know for sure. I also need to make sure that ESP32Servo takes care of the timing for sending the servo data. If it doesn't it might be your issue. If does, well, I'm not sure yet.
 
ctmal said:
I'll look over it more tomorrow or Friday but the first thing I notice is that you do a lot of waiting for data coming in. This means that the loop is pause and possibly information is not going out to the servos.
I could be wrong on that but I'll have to look at the ESP32Servo know for sure. I also need to make sure that ESP32Servo takes care of the timing for sending the servo data. If it doesn't it might be your issue. If does, well, I'm not sure yet.
Click to expand...

Thanks, Chris, but the same behavior occurs on the Arduino, but there I'm using variable speed library, so I'll try the Arduino with the standard library. However, without the header code, same behavior. When I used the debug feature, Vixen sends out data but (no interruption) the serial monitor showed the hiccups.

Take care, Joe.
 
You must log in or register to reply here.
Back
Top