Wrong servo timing

What are possible reasons for wrong timing with the servo library? The servo used to work, but now I can write any angle (0 - 180) and when I measure with the ocilloscope it's HIGH for 250 uS. If I try with the servo demo sketch I get uptimes of around 1 mS. If I add elements from my sketch to the servo demo sketch (Serial, Wire, tone, pulseIn, analogWrite) it stays well. If I disable some of those elements in my real sketch, leaving most of the remaining functionality in place, the uptime stays too short. Because the documentation for the tone function states that it messes with the timer for pins 3 and 11, I moved the servo to pin 5. The tone function is #ifdef'ed out at the moment, so it really should not be the cause, as also confirmed by testing in the servo demo sketch. But, what other possible causes should I investigate next?

I thought I could temporarily work around the problem by using servo.writeMicroseconds() with a compensation factor, but it didn't help. The HIGH time didn't change.

I’m sorry, my ouija board is at the menders at the moment - could you post your code?

It’s a bith lenghty, that’s why I didn’t post it in the first place. But here it is:

/*
IR Servo Range finder

A Sharp infrared proximity sensor mounted on a servo 
that moves back and forth, scanning the area.
*/
#define USE_COMPASS
#define USE_ACCELERO
//#define USE_BARO
//#define USE_MEASMOTOR
#define USE_RANGE_SCANNER


#include <Servo.h> 
#include <Wire.h>
#include "RangeScanner.h"
#ifdef USE_BARO
  #include "IntersemaBaro.h"
  #include "cppfix.h"
#endif

static const size_t pinIRDist     =   3; // analog
static const size_t pinLED        =  11; // needs to be PWM
static const size_t pinServo      =   5; // needs to be PWM
static const size_t pinSensorPwr  =   2; // power for compass, accelerometer and barometer
static const size_t pinMotorUpper =  12;
static const size_t pinMotorLower =   8;
static const size_t pinBaroSCLK   =   6;
static const size_t pinBaroDOUT   =   4; // in  on the arduino
static const size_t pinBaroDIN    =   9; // out on the arduino
static const size_t pinBaroMCLK   =   3; // the tone function used to generate the 34kHz signal interferes with pin 3 and 11, so it's best to use it directly
static const int    addrCompas    = (0x42 >> 1);
static const int    addrNunchuck  =  0x52;

#ifdef USE_RANGE_SCANNER
  RangeScanner  rangescn(pinIRDist, pinServo);
#endif
#ifdef USE_BARO
  Intersema::BaroPressure_MS5534C *baro = 0;
#endif
bool          doSendSerial = false;
bool          doSweepServo = false;
size_t        loopDelay    = 25;
char          tmptxt[128];

size_t readSerialString(char *txt, size_t bufsize);
void   evalCmdStr(char *txt);
 
void setup() 
{
    pinMode(pinSensorPwr, OUTPUT); 
    digitalWrite(pinSensorPwr, LOW);  
    delay(200);      
    digitalWrite(pinSensorPwr, HIGH);  
    delay(200);  
#ifdef USE_MEASMOTOR
    pinMode(pinMotorUpper, INPUT); 
    pinMode(pinMotorLower, INPUT); 
#endif
    Serial.begin(115200); // arduino bt has only this baud rate
    Wire.begin();  // join i2c bus as master
#ifdef USE_RANGE_SCANNER
    rangescn.setStepAngle(5); 
    rangescn.setScanningRange(160);
    rangescn.moveToIdlePos();
#endif
#ifdef USE_ACCELERO
    Wire.beginTransmission(addrNunchuck);// transmit to device 0x52
    Wire.send(0x40);// sends memory address
    Wire.send(0x00);// sends a zero, requesting the first data
    Wire.endTransmission();// stop transmitting    delay(100);
#endif
#ifdef USE_BARO
    baro     = new Intersema::BaroPressure_MS5534C(pinBaroMCLK, pinBaroSCLK, pinBaroDIN, pinBaroDOUT);
#endif
}
 
void loop() 
{
#ifdef USE_RANGE_SCANNER
    const int proximityAngle = rangescn.curr_pos();
    const int proximityValue = rangescn.raw_measure();
    if(doSweepServo)
        rangescn.moveNextPos(); // move to the next scan position
#endif

#ifdef USE_ACCELERO
    Wire.requestFrom(addrNunchuck, 6);
    int cnt=0;
    uint8_t buf[6];
    while(Wire.available() && cnt < 6) 
    {
        char cc = Wire.receive();
        cc = (cc ^ 0x17) + 0x17;
        buf[cnt++] = cc;
    }
    
    int accelX = buf[2];
    accelX *= 4;
    if((buf[5] >> 2) & 1) 
        accelX += 2;
    if((buf[5] >> 3) & 1)
        accelX += 1;
    int accelY = buf[3];
    accelY *= 4;
    if((buf[5] >> 4) & 1) 
        accelY += 2;
    if((buf[5] >> 5) & 1)
        accelY += 1;
    int accelZ = buf[4];
    accelZ *= 4;
    if((buf[5] >> 6) & 1) 
        accelZ += 2;
    if((buf[5] >> 7) & 1)
        accelZ += 1;
#endif

#ifdef USE_COMPASS
    Wire.beginTransmission(addrCompas);
    Wire.send('A');  // command sensor to measure angle  
    Wire.endTransmission();  
    
    // step 2: wait for readings to happen 
    delay(10);                        // datasheet suggests at least 6000 microseconds 
  
    // step 3: request reading from sensor 
    Wire.requestFrom(addrCompas, 2);       // request 2 bytes from slave device #33 
 
    // step 4: receive reading from sensor 
    int compassHeading = 0;
    if(Wire.available() >= 2)         // if two bytes were received 
    { 
        compassHeading = Wire.receive(); // receive high byte (overwrites previous reading) 
        compassHeading = compassHeading << 8;       // shift high byte to be high 8 bits 
        compassHeading += Wire.receive();    // receive low byte as lower 8 bits 
        compassHeading /= 10;
        compassHeading -= 90;  // mounting position on the board
        while(compassHeading < 0)
            compassHeading += 360;
        while(compassHeading > 360)
            compassHeading -= 360;
    } 
#endif

#ifdef USE_BARO
    const int altitude = baro->getHeightCentiMeters();
#endif

#ifdef USE_MEASMOTOR
    const size_t motorInUpper = pulseIn(pinMotorUpper, HIGH, 500);
    const size_t motorInLower = pulseIn(pinMotorLower, HIGH, 500);
#endif

#ifdef USE_ACCELERO
    Wire.beginTransmission(addrNunchuck);// transmit to device 0x52
    Wire.send(0x00);// sends one byte
    Wire.endTransmission();// stop transmitting
#endif
    
#ifdef USE_RANGE_SCANNER
    const int outputValue = map(proximityValue, 50, 700, 0, 255);
    analogWrite(pinLED, outputValue); 
#endif
    
    if(readSerialString(tmptxt, sizeof(tmptxt)))
        evalCmdStr(tmptxt);
    
    if(doSendSerial)
    {
#ifdef USE_RANGE_SCANNER
        sprintf(tmptxt, "RA%dRV%d", proximityAngle, proximityValue);
        Serial.print(tmptxt);
#endif
#ifdef USE_ACCELERO
        sprintf(tmptxt, "AX%dAY%dAZ%d", accelX, accelY, accelZ);
        Serial.print(tmptxt);  
#endif
#ifdef USE_COMPASS
        sprintf(tmptxt, "CH%d", compassHeading);
        Serial.print(tmptxt);
#endif
#ifdef USE_BARO
        sprintf(tmptxt, "BH%d", altitude);
        Serial.print(tmptxt); 
#endif
#ifdef USE_MEASMOTOR
        sprintf(tmptxt, "MU%dML%d", motorInUpper, motorInLower);
        Serial.print(tmptxt); 
#endif
        Serial.print("\n");
    }
    
    delay(loopDelay); 
    
}

size_t readSerialString(char *txt, size_t bufsize)
{
    if(!Serial.available()) 
        return 0;

    delay(10);  // wait a little for serial data

    memset(txt, 0, bufsize); // set it all to zero
    size_t i = 0;
    size_t waitCycles = 0;
    while(i < bufsize) 
    {
        if(Serial.available())
        {
            char cc = Serial.read();
            if(cc == '\n' || cc == '\0')
                break;
            txt[i++] = cc;
        }
        else if(waitCycles++ > 20)
            break;
        else
            delay(10);       
    }
    txt[min(i, bufsize-1)] = '\0';  // terminate the string
    
    return i;  // return number of chars read
}

void evalCmdStr(char *txt)
{
    if(!strcmp(txt, "lon"))
        doSendSerial = true;
    else if(!strcmp(txt, "loff"))
        doSendSerial = false;
#ifdef USE_RANGE_SCANNER
    else if(!strcmp(txt, "swon"))
        doSweepServo = true;
    else if(!strcmp(txt, "swoff"))
    {
        doSweepServo = false;
        rangescn.moveToIdlePos();
    }
    else if(!strncmp(txt, "swst", 4))
        rangescn.setStepAngle(atoi(txt + 4));
    else if(!strncmp(txt, "swrg", 4))
        rangescn.setScanningRange(atoi(txt + 4));
#endif
    else if(!strncmp(txt, "lode", 4))
    {
        int newval = atoi(txt + 4);
        if(newval >= 10 && newval <= 1000)
            loopDelay = newval;
    }
    else
    {
        static const char helpstr[] =
            "\nIrServoRangeScanner\n"
            "'lon' start sending serial data\n"
            "'loff stop' stop sending serial data\n"
            "'swon' start moving the servo with the ir proximity sensor\n"
            "'swoff' stop moving the servo with the ir proximity sensor\n"
            "'swst<nn>' angle degrees per step\n"
            "'swrg<nn>' scanning range\n"
            "'lode<nn>' milliseconds to sleep on every loop\n"
            "'?'  for this help msg\n\n"
            ; 
        Serial.print(helpstr);
        
        if(strlen(txt) && strcmp(txt, "?"))
        {
            Serial.print("Invalid option: \"");
            Serial.print(txt);
            Serial.println("\"");
        }
    }    
}

RangeScanner.h

#ifndef RANGE_SCANNER_H
#define RANGE_SCANNER_H

#include <Servo.h> 
#include <WProgram.h>

class RangeScanner
{
public:
    RangeScanner(size_t pinIrDist, size_t pinServo);
    
    int curr_pos() const { return servoPosDeg_; }
    size_t raw_measure() const;
    size_t distance_cm() const;
    
    void moveNextPos();
    void moveToIdlePos();
    void setIdlePos(const int idlePosDeg);
    void setStepAngle(const size_t newStepDeg);
    void setScanningRange(const int deg);
    void setScanningRange(const int minPosDeg, const int maxPosDeg);

private:
    Servo myservo_; 
    const size_t pinIrDist_;
    const size_t pinServo_;
    int servoMinPosDeg_;
    int servoMaxPosDeg_;
    int servoIdelPosDeg_;
    int servoStepDeg_;
    int servoPosDeg_;
    bool   turnClockwise_;
    
};

#endif // RANGE_SCANNER_H

And here is RangeScanner.cpp

#include "RangeScanner.h"


RangeScanner::RangeScanner(size_t pinIrDist, size_t pinServo)
  : pinIrDist_(pinIrDist), pinServo_(pinServo), servoMinPosDeg_(45), 
    servoMaxPosDeg_(135), servoStepDeg_(10), servoIdelPosDeg_(90), 
    servoPosDeg_(servoIdelPosDeg_), turnClockwise_(false)
{
    myservo_.attach(pinServo_);
}

size_t RangeScanner::raw_measure() const
{
    int analogValue = analogRead(pinIrDist_);
    return analogValue;
}

size_t RangeScanner::distance_cm() const
{
    // thanks to luckylarry.co.uk for the easy calculation
    const float volts  = analogRead(pinIrDist_) * 5.0 / 1024;
    const float dist_m = 65 * pow(volts, -1.1);
    return dist_m * 100;
}
    
void RangeScanner::moveNextPos()
{
    if(servoPosDeg_ < 0 || servoPosDeg_ > 180)
        servoPosDeg_ = 90;
        
    if(turnClockwise_)
    {
        servoPosDeg_ -= servoStepDeg_;
        if(servoPosDeg_ <= servoMinPosDeg_)
            turnClockwise_ = false;
    }
    else
    {
        servoPosDeg_ += servoStepDeg_;
        if(servoPosDeg_ >= servoMaxPosDeg_)
            turnClockwise_ = true;
    }
    //myservo_.write(servoPosDeg_);
    const size_t usec = map(servoPosDeg_, 0, 180, 1000, 2000);
    myservo_.writeMicroseconds(usec * 16);
}

void RangeScanner::moveToIdlePos()
{
    servoPosDeg_ = servoIdelPosDeg_;
    //myservo_.write(servoPosDeg_);
    const size_t usec = map(servoPosDeg_, 0, 180, 1000, 2000);
    myservo_.writeMicroseconds(usec * 16);
}

void RangeScanner::setIdlePos(const int idlePosDeg)
{
    if(idlePosDeg >= 0 && idlePosDeg <= 180)
        servoIdelPosDeg_ = idlePosDeg;
}

void RangeScanner::setStepAngle(const size_t newStepDeg)
{
    if(newStepDeg > 0 && newStepDeg <= (servoMaxPosDeg_ - servoMinPosDeg_) / 2)
        servoStepDeg_ = newStepDeg;
}

void RangeScanner::setScanningRange(const int deg)
{
    const int halfrng = deg / 2;
    servoMinPosDeg_ = servoIdelPosDeg_ - halfrng;
    servoMaxPosDeg_ = servoIdelPosDeg_ + halfrng;
}

void RangeScanner::setScanningRange(const int minPosDeg, const int maxPosDeg)
{
    servoMinPosDeg_ = minPosDeg;
    servoMaxPosDeg_ = maxPosDeg;
}

The servo that has problems in in the range scanner.

Ok, I found the problem with the short pulses. If I create the RangeScanner on the heap, the servo works again, at least in a small toy sketch. But on my working sketch, some values like e.g. servoPosDeg_ get invalid values. I thought that this could be a problem of variables getting overwritten. That was why I was trying to allocate the object on the stack. I suspect the problem is that the "myservo_.attach(pinServo_);" in the RangeScanner constructor is executed before the setup() function.

So, now I have a few general questions: 1) How well is stack allocation supported on the arduino? To be able to use the new operator, I had to use the code below, which I copied from somewhere. I had the impression, that the binary size increased when using this, which is not surprising, knowing that memory managers are not the easiest thing. Does it use the implementation of the std c lib, or is it different?

#ifndef CPPFIX_H
#define CPPFIX_H

// include this file of you want to use c++ in arduino processing without linking to the c++ std lib

__extension__ typedef int __guard __attribute__((mode( __DI__)));

void * operator new (size_t size);
void   operator delete (void *ptr);

int  __cxa_guard_acquire(__guard *g) {return !*(char*)(g); };
void __cxa_guard_release(__guard *g) { *(char*)g = 1; };
void __cxa_guard_abort(__guard *)    { };

void * operator new(size_t size)
{
    return malloc(size);  
}

void operator delete(void *ptr)
{
    free(ptr);
}
#endif

2) How is the memory organized on the arduino? What do I have to look for if I suspect that it's running out of memory? Obviously there is no message box, and if there was an exception thrown which I don't catch, then the rest of the sketch would not work.

3) I find it hard to combine sound c++ class design with the processing framework. Especially, I want my object to be ready to use after construction and have all invariants initialized as constants. Usually in the spirit of RAII that means declare and initialize an object in the narrowest possible scope. That's where the problem lies. If I initialize the object before the setup function, I can use it from everywhere, but I ran into the problem with the servo library. If I declare it before and initialize it on the heap in the setup function, I have to use the possibly expensive malloc and have a pointer where I would prefer not to. With the current example I might add an initialize() function to the RangeScanner class, that would have to be called in setup(), but that's against the RAII spirit. If one forgets to call initialize() he might run into undefined behaviour. Or I could have an internal boolean that tells if the object was initialized, that is checked on every call, and on the first one does the initialisation. That would be the safest, but its bad for performance and readability. So, what are your experiences?

The typical Arduino library philosophy is to do nothing much in the constructor. Instead, most libraries have a begin method, where all the stuff that one would normally do in the constructor gets done (Serial.begin(), for example).

Since myservo_ is an instance of the Servo class, the myservo_.attach() call may be being made before the Servo::Servo() call is complete. That would, of course, be a problem.

By moving myservo_.attach() to a method that is called after the RangeScanner::RangeScanner() call is complete, the Servo::Servo() call is guaranteed to be complete, too, so the myservo_.attach() function can operate correctly, and perform the required actions.

I'm not suggesting that there isn't a flaw in the Arduino's handling of classes, but the use of a begin method is how the flaw is usually worked around.

Ok, I added a begin function, that is called in setup() and calls myservo_.attach().
Although my experience tells me, that member variables are constructed before entering the class’s constructor.
Anyway, I put the RangeScanner instance onto the stack again, and the thing is working.
I can’t say I like the solution too much, but now the code is probably a better match to my soldering :wink:

I can't say I like the solution too much, but now the code is probably a better match to my soldering

Hopefully, both will improve over time. :)

After enabling more features, my arduino again started to show strange behaviour. All features ran well in isolation, but when I compiled them into the same binary, I got strange results. This smelled like overwritten memory, but there was no obvious indication other than that. With a bit of googling I found a very good article about memory organization and optimization on the arduino: http://itp.nyu.edu/~gpv206/2008/04/making_the_most_of_arduino_mem.html After applying some of the techniques, it's working with all the features without the strange behavior.

So, isn't there any indication, be it an interrupt or anything, that would indicate out of memory? That would be very helpful. I mean, if I don't use the heap, it should be possible to detect the stack pointer approaching the physical limit? And what exactly happens beyond? A simple overflow?

Found the answers to my questions here http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1236343710 and here http://www.gnu.org/savannah-checkouts/non-gnu/avr-libc/user-manual/malloc.html