After making the snake using the Script Block I thought I would give it a try using the logic maker instead. Since there isn’t an easy way to use variables in the logic maker I pre-calculated the variables for the serpenoid curve instead.
Snake movement algorithm calculations
If you don’t want to mess with the parameters just copy and paste this into the FORMULA Gate.
If you want to play with the parameters you can calculate the variables below
A - affects the amplitude of the curve. I used 1.5
B - changes the number of periods. I used 2
C - allows for steering of the curve. I used a value of 0 so it will go straight.
n - number of segments in the snake robot. I used 7
The diagram below shows graphically how the A, B and C values change the curve.
(NOTE: you will be limited by the physical constraints of your snake. If your snake is only 5 segments long there is no way you will able to create a shape with many periods and a large amplitude. It’s the same concept as trying to make a sine wave with peaks of 5cm with a 2cm long piece of rope.)
With the A,B and C values set you can calculate the alpha beta and gamma parameters.
Gamma = -C/n
Beta = B/n
Alpha = A*sin(Beta/2)
Offset - since the formula calculates the angle in reference to the next link, it gives values ranging from -90 to +90 degrees. To convert this to a value the servo motors can use, an offset of 90 degrees is added.
Now that you have all your parameters substitute them into the formula below to calculate the angle that each servo should be set at.
Angle = Alpha * Sin(t-i*beta)*180/pi + gamma + offset
i - The variable i is set to the number of the servo you are currently calculating the angle for.
t - t is the time interval. It can range from 0-2pi.
For the build of the robot you will need two MotorDriver Blocks. Using the Logic Maker for the recipe below was created:
How the logic works is that there is a loop over the servo selector, which will feed in the number of the servo to the angle calculation one by one. The then calculated servo angles are then fed through to that specific servo using the SEQUENCE Gate. This loops around 8 times before incrementing the time interval, which then starts the process again with the new incremented time. This will continue to repeat, moving the servos in a serpentine wave.
Set time Interval
The greater you make the interval the faster your snake will go, however there is a limit to the maximum increment. This increment value is the sampling rate of your sin curve. If your sampling rate is too big you will not have a true representation of a sine curve and therefore your snake will not move as expected. This is called aliasing.
Feeds in the next servo number into the FORMULA Gate that calculates the next angle.
Calculate Next Angle
Calculates the next angle using the servo number and the time increment.
Send Angle to Servo
Using the SEQUENCE Gate the next angle is sent to each servo. Since it will follow a sequence as each angle is calculated it will send it to the corresponding servo motor. At the end of the 8th motor it increments the time interval so that the positions can be recalculated.