wave moving down line of servos

Hey - I’m having trouble figuring out how to get servos to undulate - What I’d like is an index of values that will propagate through a line of servos, to create an undulation effect (I’m making a robotic snake). I’m having loads of trouble getting this to happen . I want control of the speed of the propagation and if possible, the timing delay from one to another, from outside input.

I have a class called servoWave

constructor: ServoWave::ServoWave(int _numServos, float _angle, unsigned long _period, int _wavelength, int _ampMax){

the number of servos is the total number of servos in the body, the angle is the angle the servo should be written at, the period corresponds to the delay period, the wavelength corresponds to the number of servos until the value is reset to 0, and the ampMax is the total amplitude that the servos will be at.

So I have an index of values that I’m writing the wave to be:

when I’m printing out the angles for each servo, I’m just getting 90 for wave , and my angle equals 0, instead of a whole array of values that are undulating.
I’m not sure what I’m doing wrong, any help would be appreciated
#include “servoWave.h”
//constructor with argiuments about the wave
ServoWave::ServoWave() {
ServoWave::~ServoWave() {

  • delete servoPositions;*
  • delete servos;*
    ServoWave::ServoWave(int _numServos, float _angle, unsigned long _period, int _wavelength, int _ampMax){
  • numServos = _numServos;//servos of the whole unit*
  • angle = _angle;//height of wave*
  • period = _period; //in milliseconds*
  • wavelength = _wavelength;//how many servos long the wave will be*
  • ampMax=_ampMax;*
    float p = 0;
  • int total = int(TWO_PI/0.01);//total num of index pioints, until the amplitude is back at zero*
  • int numServos = _numServos;*
  • //set currentServo to 0*
  • int currentServo = 0;*
  • int lastPosition=-1;*
  • int steps = 5;*
  • long previousMillis = 0;*
  • long lastUpdatedAt = millis();*
  • float wave[total];*
  • p = 0;*
  • //initialize servoPosition array*
  • for(int i=0;i<numServos;i++){//wtf*
    _ servoPositions = 90; // starting position for all servos. could be passed as an argument_
    * }*
    void ServoWave::addServo(Servo servo){
    * servos[currentServo]=servo;*
    * currentServo++;*
    void ServoWave::addServo(Servo servo, int startingPosition){
    * servos[currentServo]=servo;*
    * servoPositions[currentServo] = startingPosition;*
    * currentServo++;*
    void ServoWave::update(){

for(int i=0;i<total;i++){
wave = map(sin(p), -1,1,0,180);
for(int i=0;i<numServos;i++){
void ServoWave::move(){
for(int i=0;i<numServos;i++){
angle = wave[index*];*
index = (index*+1)%total;*
int ServoWave::calculateAngle(){
void ServoWave::passDownValues(int firstValue){

void ServoWave::adjustServos(int curA){
servosHatt__DYNAMIC_NEWNEW.zip (3.03 KB)

Hi, The function map takes a long as its input so it is rounding your value p to either 1 or 0, easy to fix, multiply p by 100 and change the following -1,1 to -100,100.

Duane B