Efficiently moving a servo while making multiple measurements

Hello all,
I am working on a project that requires I move a servo while reading the voltage, current, angle (from a potentiometer), and data read in from an accelerometer. My basic problem is that I can’t figure out a way to move the servo while reading and printing 4 separate variables (I’m using Arduino Uno). I must print each sample to the serial board as this is the only way I’m currently reading the data into MATLAB, which is vital to my project. Despite the tremendous amount of research I’ve done, I have surprisingly not found anything online that can help me accomplish this. However, I am new to Arduino and am likely unaware of many features.
Below is all my code. Before, I attempted to read the data at a decent sampling rate (which I consider to be at least 200 Hz) using the analogueread() function, but the servo noticeably slowed down to an unacceptable level. Currently I am using ISR, yet when I try to read from multiple pins I have residue readings from one pin into another.

Is it possible to move a servo without hindrance and still make accurate, multiple measurements (with the highest sampling rate possible) on a single Arduino? If not, would this be possible with two Arduinos? i.e., have one Arduino move the servo while another reads in data. If so, how would I implement this? Moreover, do I require a more powerful Arduino, such as the Mega? Are my print statements causing issues?

Any help would be immensely appreciated.

#include<Servo.h>
Servo myservo;
volatile int aval = 0;      // analog value


void setup() { 
  DIDR0 = 0x3F;            // digital inputs disabled
  ADMUX = 0x40;            // measuring on ADC, use the internal 1.1 reference
  ADCSRA = 0xAC;           // AD-converter on, interrupt enabled, prescaler = 16
  ADCSRB = 0x40;           // AD channels MUX on, free running mode
  bitWrite(ADCSRA, 6, 1);  // Start the conversion by setting bit 6 (=ADSC) in ADCSRA
  sei();                   // set interrupt flag
  
  myservo.attach(11);
  Serial.begin(9600);
  //time1= millis();
}

int minposition = 1200;    // upper angle (1200) //adjust this value smaller to find start end position by 5 usec steps
int maxposition = 1950;   //  lower angle (1950) //adjust this value larger to find stop end position by 5 uses steps
int pos= 0;
void loop() { 

  for(pos = minposition; pos < maxposition; pos += 11)  // move in 5 usec steps 
  {      
    myservo.writeMicroseconds(pos);    // tell servo to go to position in variable 'pos' 
    delay(10);
    ADC_select_pin(0);
    ADC_select_pin(1);
    ADC_select_pin(2);
  } 
  Serial.println(-10);//Serial.print('\t'); Serial.println(time_lapsed);
  delay(1000);

  
  for(pos = maxposition; pos>= minposition; pos-= 11)     // move in 5 usec steps 
  {                                
    myservo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos' 
    delay(10);
  } 
  Serial.println(-10);                          //Serial.print('\t'); Serial.println(time_lapsed);
  delay(1000);
}


void ADC_select_pin(int pin){
 if (pin == 0){
   ADMUX= 0x40; //ADC1, pin 0
   
   //Serial.print("pin 0 gets:"); 
   Serial.print(aval); Serial.print('\t');
   delay(0.05);
 } 
 if (pin== 1){
   ADMUX= 0x41;// ADC2, pin1
   
   //Serial.print("pin 1 gets:"); 
   Serial.print(aval);Serial.println('\t');
   delay(0.05); 
 }
 if (pin== 2){
   ADMUX= 0x42; //ACD 3, pin 2
   
   //Serial.print("pin 2 gets:"); 
   Serial.print(aval);Serial.print('\n');
   delay(0.05); 
 }
}

/*** Interrupt routine ADC ready ***/
ISR(ADC_vect) {

  aval = ADCL;        // store lower byte ADC
  aval += ADCH << 8;  // store higher bytes ADC

}

using the analogueread() function

You want to post a link to the documentation for that function?

  for(pos = minposition; pos < maxposition; pos += 11)  // move in 5 usec steps

Five is NOT equal to 11. Useless comments MUST be kept correct.

   delay(0.05);

Back to the documentation for you. The delay() function takes an INTEGER value.

 if (pin == 0){
   ADMUX= 0x40; //ADC1, pin 0
   
   //Serial.print("pin 0 gets:"); 
   Serial.print(aval); Serial.print('\t');
   delay(0.05);
 }

In all three blocks, ADMUX is equal to 0x40 + the value in pin. Three separate blocks are NOT necessary.

    delay(10);

I must have misread something above. I could have sworn that you said speed was important. I guess not.

If you want things to work smoothly don't use the delay() function.
Also, don't use a FOR loop to move your servo.

Look at how timing is managed using millis() in the demo several things at a time. It also shows how to move the servo without blocking everything else.

...R

Hi PaulS and Robin2,

Thanks for the response. Your comments are really helpful, and I think I can solve all my problems with your help.

You want to post a link to the documentation for that function?

Sorry about that. I meant analogRead().

Five is NOT equal to 11. Useless comments MUST be kept correct.

I apologize for this as well. I’ll make sure from now on that comments represent the most updated version of my code.

Back to the documentation for you. The delay() function takes an INTEGER value.

Although I never got an error from arduino IDE, I wanted to place a brief delay to prevent residue readings from the ADC.

In all three blocks, ADMUX is equal to 0x40 + the value in pin. Three separate blocks are NOT necessary.

Thanks a lot for pointing that out. I made the changes in the code, and I think its more optimized now.

I must have misread something above. I could have sworn that you said speed was important. I guess not.

You’re completely right, I’m adding a delay because I need to be able to control the speed of the servo. Right now, I can’t think of any other way to do this. Moreover, from my understanding, the delay makes it so that two consecutive command pulses to the servo do not overlap. If this is not a good idea, or if the delay is too long, please let me know. Overall, I’m trying to make the servo move as fast as possible.

To investigate this further, I’ve implemented changes based on your comments as well as Robin2’s, using millis() instead of delay() (see the code below). However, here’s the main issue I’m facing:

Right now, my script calls consecutive functions when attempting to read from different pins. I am outlining the code based on the link Robin2 provided. However, when looking at the data I’m collecting, it is not an accurate representation of the respective signals. I believe I am not sampling fast enough. In this line of code:

if (currentMillis - previousServoMillis >= servoInterval)

I believe the condition is being met in every iteration of the void loop, and therefore the number of samples I obtain equals (servoMaxDegrees-servoMinDegrees)/servoDegrees. In this case it’s (1950-1200)/11, which equals 68 samples. I copied the data printed in the serial board into Excel and confirmed this to be true.

In contrast, I tried increasing servoInterval (the period in between pulse commands), and the condition:

if (currentMillis - previousServoMillis >= servoInterval)

becomes false several times in the void loop, which increased my sampling rate but significantly slowed down the servo.

Overall, I want to be able to move the servo as fast as possible, while reading data from 4 different pins at a high sampling rate. Is this possible?

// ----------LIBRARIES--------------

#include <Servo.h>

// --------CONSTANTS (won't change)---------------

const int servoPin = 11; // the pin number for the servo signal

const int servoMinDegrees = 1200; // the limits to servo movement
const int servoMaxDegrees = 1950;


//------------ VARIABLES (will change)---------------------

Servo myservo;  // create servo object to control a servo 

int servoPosition = 1250;     // initial angle/position of servo
int servoSlowInterval = 80; // millisecs between servo moves
int servoFastInterval = 10;
int servoInterval = servoSlowInterval; // initial millisecs between servo moves
int servoDegrees = 11;       // amount servo moves at each step                             
int aval = 0;      // analog value

unsigned long currentMillis = 0;    // stores the value of millis() in each iteration of loop()

unsigned long previousServoMillis = 0; // the time when the servo was last moved


//========================================

void setup() {

  Serial.begin(57600); //fastest baud rate
  
  myservo.writeMicroseconds(servoPosition); // sets the initial position
  myservo.attach(servoPin);
 
  DIDR0 = 0x3F;            // digital inputs disabled
  ADMUX = 0x40;            // measuring on ADC1, use the internal 1.1 reference
  ADCSRA = 0xAC;           // AD-converter on, interrupt enabled, prescaler = 16
  ADCSRB = 0x40;           // AD channels MUX on, free running mode
  bitWrite(ADCSRA, 6, 1);  // Start the conversion by setting bit 6 (=ADSC) in ADCSRA
  sei();                   // set interrupt flag
 
}

//========================================

void loop() {
 
  currentMillis = millis();   // capture the latest value of millis()
                              //   this is equivalent to noting the time from a clock
  servoSweep();
  ADC_select_pin(0); //attempting to read data from 4 different pins
  ADC_select_pin(1);
  ADC_select_pin(2);
  ADC_select_pin(3);
  Serial.print('\n');
}



//======================================

void servoSweep() {

      // nothing happens unless the interval has expired
      // the value of currentMillis was set in loop()
  
  if (currentMillis - previousServoMillis >= servoInterval) { // the condition that may affect my sampling rate
        // its time for another move
    previousServoMillis += servoInterval;
    
    servoPosition = servoPosition + servoDegrees;

    if (servoPosition <= servoMinDegrees) {
          // when the servo gets to its minimum position change the interval to change the speed
       if (servoInterval == servoSlowInterval) { 
         servoInterval = servoFastInterval;
       }
       else {
        servoInterval = servoSlowInterval;
       }
    }

    if ((servoPosition >= servoMaxDegrees) || (servoPosition <= servoMinDegrees))  {
          // if the servo is at either extreme change the sign of the degrees to make it move the other way
      Serial.println(-10); //this print statement is a marker between servo sweeps
      
      servoDegrees = - servoDegrees; // reverse direction
          // and update the position to ensure it is within range
      servoPosition = servoPosition + servoDegrees; 
    }
    
        // make the servo move to the next position
    myservo.writeMicroseconds(servoPosition);
        // and record the time when the move happened

  }
}  


//=========================================

void ADC_select_pin(int pin){ //function to pick pins
  ADMUX= B01000000 + pin;
  Serial.print(aval);Serial.print('\t');
  
}

//==========================================

/*** Interrupt routine ADC ready ***/
ISR(ADC_vect) { 

  aval = ADCL;        // store lower byte ADC
  aval += ADCH << 8;  // store higher bytes ADC

}
//==========================================

Edit: Note that I used some of Robin2’s comments in the code for reference.

I have a hard time believing that servos move faster than millis timing can handle, given a fast loop(). If the loop() blocks even sometimes then it's not the millis causing the problem.

That stated, Arduino also provides the micros() function with a granularity of 4 microseconds.
You can shift the millis timing to micros and get 90 minute intervals using unsigned longs.

I've had a quick look at the code in Reply #3

Why are you using all that ADMUX stuff if you only need to get 200 samples per second? Will analogRead() not work that fast?

I reckon it is the Serial.print() statements that are causing you timing problems.

Can't you collect the data into an array and just send it to the PC occasionally?
You could also greatly reduce the amount of data to be sent by using Serial.write() to send binary data rather than human-readable data.

...R

Serial to PC can run at 115200 baud.
If that's not fast enough, connect another AVR via SPI and give the channel to USB to that chip/board.
OR shorten the messages using code characters and binary values as Robin suggested.

I didn't know you have to treat servos like steppers and control every step. This is learning for me.

Overall, I want to be able to move the servo as fast as possible, while reading data from 4 different pins at a high sampling rate

I am not clear what part the servo plays in your project. Does its position affect the analogue readings ?

If you want the servo to move as fast as possible stop doing it by moving the servo in steps and just give it an absolute position to go to and it will be get on with it with no delay to the program and no intervention needed by the Arduino.

Hi Robin2,

Thanks for your help. You were right, it was the printing that was messing up the code. As for analogRead(), it was a tad too slow to measure what I needed. I also did what you suggested, and am now saving the data into arrays which get printed at the end of the sweep. This has worked wonders for me, however, with the new version of the code, I am unable to obtain a time vector. Essentially, every time I make a measurement, I also wanted to measure the time. For example, I am measuring the angle/position of the servo. In order to eventually calculate velocity, I need a time vector. Below is a sample of code I’ve written that simply doesn’t function. When implementing this into the servo sweep function, the servo does not run. I believe this specific line of code could be presenting a problem:

timevector[index]= currentMillis;

I also believe there’s a fundamental issue with my logic. Would you be able to take a look? Further below is my servo sweep function with the time vector code commented out. As I said earlier, it appears to be working great, but let me know if you see any other glaring issues.

Code to try and measure the time into an array:

unsigned long currentMillis = 0;  

int index= 0;
unsigned long timevector[1000];

void setup() {

  Serial.begin(57600);
 
}


void loop() {


  currentMillis = millis();   
  
  if (index <= 1000){
    timevector[index]= currentMillis;
    index += 1;
  }
  else{
    printarray();
  }
}

void printarray(){
  for(int i =1; i <= 1000; i+=1 ){
    Serial.println(timevector[i]);
  }
}

Here is our current code for moving the servo and measuring from 3 pins:

// ----------LIBRARIES--------------

#include <Servo.h>

// --------CONSTANTS (won't change)---------------

const int servoPin = 11; // the pin number for the servo signal

const int servoMinDegrees = 1200; // the limits to servo movement
const int servoMaxDegrees = 1950;


//------------ VARIABLES (will change)---------------------

Servo myservo;  // create servo object to control a servo 

int servoPosition = 1250;     // the current angle of the servo - starting at 90.
int servoSlowInterval = 20; // millisecs between servo moves
int servoFastInterval = 10;
int servoInterval = servoSlowInterval; // initial millisecs between servo moves
int servoDegrees = 11;       // amount servo moves at each step 
                            //    will be changed to negative value for movement in the other direction
                            
int aval = 0;      // analog value

unsigned long currentMillis = 0;    // stores the value of millis() in each iteration of loop()

unsigned long previousServoMillis = 0; // the time when the servo was last moved

unsigned long timevector[1000];

int samplingrate= 200;

int array0[200]; int array1[200]; int array2[200];
int index= 0;

int i;
//=================================================================================================

void setup() {

  Serial.begin(57600);
  Serial.println("Starting SeveralThingsAtTheSameTimeRev1.ino");  // so we know what sketch is running
  
  myservo.writeMicroseconds(servoPosition); // sets the initial position
  myservo.attach(servoPin);
 
  DIDR0 = 0x3F;            // digital inputs disabled
  ADMUX = 0x40;            // measuring on ADC, use the internal 1.1 reference
  ADCSRA = 0xAC;           // AD-converter on, interrupt enabled, prescaler = 16
  ADCSRB = 0x40;           // AD channels MUX on, free running mode
  bitWrite(ADCSRA, 6, 1);  // Start the conversion by setting bit 6 (=ADSC) in ADCSRA
  sei();                   // set interrupt flag
 
}

//===================Main Loop=======================================================================

void loop() {

      // Notice that none of the action happens in loop() apart from reading millis()
      //   it just calls the functions that have the action code

  currentMillis = millis();   // capture the latest value of millis()
                              //   this is equivalent to noting the time from a clock

//  if (index <= 1000){
//    timevector[index]= currentMillis;
//    index += 1;
//  }
  
  
  ADC_select_pin(0); 
  ADC_select_pin(1);
  ADC_select_pin(2);
  
  servoSweep();
  
}



//===========Servo Sweep function======================================================================

void servoSweep() { //

      // nothing happens unless the interval has expired, the value of currentMillis was set in loop()
  if (currentMillis - previousServoMillis >= servoInterval) {
        // its time for another move
    previousServoMillis += servoInterval; // updating previousServoMillis
    
    servoPosition = servoPosition + servoDegrees; // servoDegrees might be negative
    
    
//=======Ramping up and Down==============================//
    if (servoPosition <= servoMinDegrees) {
          // when the servo gets to its minimum position change the interval to change the speed
       if (servoInterval == servoSlowInterval) {
         servoInterval = servoFastInterval;
       }
       else {
        servoInterval = servoSlowInterval;
       }
    }
//=======Turning around & printing=======================//
    if ((servoPosition >= servoMaxDegrees) || (servoPosition <= servoMinDegrees))  {
          // if the servo is at either extreme change the sign of the degrees to make it move the other way

      servoDegrees = - servoDegrees; // reverse direction
          // and update the position to ensure it is within range
      servoPosition = servoPosition + servoDegrees; 
      
      print_arrays(0);
      print_arrays(1);
      print_arrays(2);
      
// ====== Printing time vector===========================//     
//      for (i = 1; i < 1000; i += 1){
//        Serial.print(timevector[i]);
//      }

      Serial.println(-10);
      delay(1000);
    }
    
        // make the servo move to the next position
    myservo.writeMicroseconds(servoPosition);
        // and record the time when the move happened

  }
}  


//==========Printing arrays function======================

//Here I got my pins mixed up...

void print_arrays(int pin){
  if (pin == 0){
    Serial.println("pin 1");
    for (i = 1; i < samplingrate; i += 1){
      Serial.println(array0[i]);
    }
  }
  
  if (pin == 1){
    Serial.println("pin 2");
    for (i = 1; i < samplingrate; i += 1){
      Serial.println(array1[i]);
    }
  }
  
  if (pin == 2){
    Serial.println("pin 0");
    for (i = 1; i < samplingrate; i += 1){
      Serial.println(array2[i]);
    }
  }
  
}

//================Data collection function====================

void ADC_select_pin(int pin){
  //ADMUX= B01000000 + pin;
  if (pin == 0){
    ADMUX= 0x40;

    for (i = 1; i < samplingrate; i += 1){
      array0[i]= aval;
    }
  }
  if (pin == 1){
    ADMUX= 0x41;

    for (i = 1; i < samplingrate; i += 1){
      array1[i]= aval;
    }
  }
  if (pin == 2){
    ADMUX= 0x42;

    for (i = 1; i < samplingrate; i += 1){
      array2[i]= aval;
    }
  } 
}

//=================Interrup Service Routine=====================

ISR(ADC_vect) {

  aval = ADCL;        // store lower byte ADC
  aval += ADCH << 8;  // store higher bytes ADC

}
//=========================================================

UKHeliBob,

Thanks for your suggestion. Moving the servo to an absolute position is a great idea, but I can’t think of how I’d be able to control or vary its speed other than increasing the delay between pulses, which necessitates an incremental update of the position. As for what I’m actually trying to do, my end goal is to measure the instantenous torque of the servo. I currently plan on doing this by reading in the current, position, time, voltage, and acceleration (via an accelerometer) of the servo. Let me know if that was clear, or if you need any more information. Thanks a lot for your help regardless.

unsigned long timevector[1000];

4000 bytes of SRAM on which Arduino?

Arduino Uno. Can it not handle that? I also have a microSD shield add on if that's useful.

Can it not handle that?

The UNO has 2048 bytes of SRAM. You do the math.

I also have a microSD shield add on if that's useful.

Maybe. There will be periodic latency issues, though, as the buffer gets full and needs to be committed to the card.