0

I have code that receives values from a serial connection, then uses those values to generate sine wave points. These sine wave points are used to position multiple servos.

I am currently trying to decrease the resolution of the sine wave points so there is more time for the servo to reach its given position.

The only way I can think of to accomplish this is by increasing the time between writing servo positions. As far as I can tell, I have implemented this correctly, however it results in a sine wave of a lower frequency.

The method I am using to alter the resolution is by changing the variable sine_time, which is used in the conditional statement if(currentMillis - previousMillis >= sine_time), near the bottom of the program.

/*
This is a program that recieves sine wave parameters from a serial connection in a single string.
The string is parsed and converted into numbers, which are plugged into 8 sine equations to generate
positions for each joint of the robot.
*/

#include <Servo.h>

Servo SB, SF, HB, HF, EB, EF, KB, KF;
int iAmp, iF_offset, iB_offset, iFk_offset, iBk_offset, pos[] = {0,1,2,3,4,5,6,7};
unsigned int t = 0;
unsigned long previousMillis = 0;
float fKnee_amp, fAfreq, fPhase, fGait;

void setup(){
  //Start serial stream at 115200 baud
  Serial.begin(115200);

  //Connect servos to Arduino
  HB.attach(2);
  KF.attach(3);
  SB.attach(4); 
  EB.attach(5);
  HF.attach(6); 
  KB.attach(7);
  SF.attach(8); 
  EF.attach(9);

  delay(3000);
}

void loop(){
  char c, buffer[10];
  String readString, amp, knee_amp, afreq, gait, phase, f_offset, b_offset, fk_offset, bk_offset;
  //change the resolution of the sine wave (lower value is higher resolution)
  //digital servos refresh at a rate of 50Hz which is 0.02s
  float sine_time = 100;

  while (Serial.available() && readString.length() < 37){
    //delay to allow buffer to fill
    delayMicroseconds(10);
    if (Serial.available() > 0){
      //get one byte from serial buffer
      char c = Serial.read();
      //creates the string "readString"
      readString += c;
    }
  }

  if (readString.length() > 36){
    //see what was received
    Serial.println("The string is: " + readString);
    Serial.println(readString.length()); 

    //parse serial string into separate parameters
    amp = readString.substring(0, 3);
    knee_amp = readString.substring(3, 7); 
    afreq = readString.substring(7, 11); 
    phase = readString.substring(11, 15); 
    gait = readString.substring(15, 20); 
    f_offset = readString.substring(20, 24); 
    b_offset = readString.substring(24, 28); 
    fk_offset = readString.substring(28, 32); 
    bk_offset = readString.substring(32, 36);
    /*
    //Print to serial monitor to see parsed results
    Serial.println("Hip Amp: " + amp); 
    Serial.println("Knee Amp: " + knee_amp); 
    Serial.println("Afreq: " + afreq); 
    Serial.println("Gait: " + gait);
    Serial.println("Phase: " + phase);
    Serial.println("F Offset: " + f_offset);
    Serial.println("B Offset: " + b_offset);  
    Serial.println("Fk Offset: " + fk_offset);
    Serial.println("Bk Offset: " + bk_offset + "\n");
    */
    //convert string values to numbers
    iAmp = amp.toInt();

    knee_amp.toCharArray(buffer, 10);
    fKnee_amp = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    afreq.toCharArray(buffer, 10);
    fAfreq = atof(buffer)/100;
    memset(buffer, 0, sizeof(buffer));

    phase.toCharArray(buffer, 10);
    fPhase = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    gait.toCharArray(buffer, 10);
    fGait = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    iF_offset = f_offset.toInt();
    iB_offset = b_offset.toInt();
    iFk_offset = fk_offset.toInt();
    iBk_offset = bk_offset.toInt();

    //clear strings
    readString="";
    amp="";
    knee_amp="";
    afreq="";
    gait="";
    phase="";
    f_offset="";
    b_offset="";
    fk_offset="";
    bk_offset="";
  }

  //if the joints should be oscillating
  if (fAfreq > 0){
    //determine joint positions
    pos[0] = iAmp * sin(fAfreq*t) + 1500 + iF_offset;
    pos[1] = iAmp * sin(fAfreq*t) + 1522 - iF_offset;
    pos[2] = iAmp * sin(fAfreq*t + fGait) + 1500 + iB_offset;
    pos[3] = iAmp * sin(fAfreq*t + fGait) + 1446 - iB_offset;

    pos[4] = iAmp*fKnee_amp*2 * sin(fAfreq*t + fPhase) + 1500 + iFk_offset;
    pos[5] = iAmp*fKnee_amp * sin(fAfreq*t + fPhase) + 1500 - iFk_offset;
    pos[6] = iAmp*fKnee_amp*2 * sin(fAfreq*t + fGait + fPhase) + 1500 + iBk_offset;
    pos[7] = iAmp*fKnee_amp*2 * sin(fAfreq*t + fGait + fPhase) + 1538 - iBk_offset;
    /*
    //print parameter variables
    Serial.println(iAmp); 
    Serial.println(fKnee_amp); 
    Serial.println(fAfreq); 
    Serial.println(fGait);
    Serial.println(fPhase);
    Serial.println(iF_offset);
    Serial.println(iB_offset);  
    Serial.println(iFk_offset);
    Serial.println(iBk_offset);

    //print servo positions
    Serial.println(pos[0]);
    Serial.println(pos[1]);
    Serial.println(pos[2]);
    Serial.println(pos[3]);
    Serial.println(pos[4]);
    Serial.println(pos[5]);
    Serial.println(pos[6]);
    Serial.println(pos[7]);
    */
    //get time in milliseconds
    unsigned long currentMillis = millis();

    //if the time threshold has been passed
    if(currentMillis - previousMillis >= sine_time){

      Serial.println(currentMillis - previousMillis);

      previousMillis = currentMillis;

      //write servo positions
      HB.writeMicroseconds(pos[2]);
      KF.writeMicroseconds(pos[6]);
      SB.writeMicroseconds(pos[0]); 
      EB.writeMicroseconds(pos[4]);
      HF.writeMicroseconds(pos[3]); 
      KB.writeMicroseconds(pos[7]);
      SF.writeMicroseconds(pos[1]); 
      EF.writeMicroseconds(pos[5]);

      //increment time parameter
      t+=1;
    }
  }
}
0

Your sine waves depend on “time” via appearances of fAfreq*t in lots of places. However, t actually is a pass counter, rather than reflecting true elapsed time.

Instead of incrementing t by 1, increment it by a value that's proportional to elapsed time. Or, more directly, set t equal to a multiple of elapsed time. For example, whenever you start over, record current time via basetime = millis(). Then, just after your //determine joint positions comment, say t = speedratio * (millis()-basetime);.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.