Program for robot

I’m working on building a robot which functions with a servo motor for steering, accelerometer for orientation, reed switch to measure distance traveled, an air solenoid in order to actuate a piston. The accelerometer value which appears on the serial monitor seems to fluctuate in a range of about 100. I’m not sure why this is happening, with the sample code that i have, which tests the function of each part, the accelerometer reading is very steady. I would appreciate it if someone could take a look at my code and critique it or help me find a solution. If this is not the correct place to pot this, I apologize, I would appreciate if if you can point me in the right direction.

 * States:
 * 1. Startup state to delay for a few seconds (say 10sec)
 * 2. The robot moves a distance x determined by the read sensor (initial/starting point)
 * 3. Read magnetometer, and turn based on (2)
 * 4. Move in that direction.

#include <Servo.h>
#include <Wire.h>
#include <LSM303.h>

// Finite states machine
enum state_t {START, STRIDE, BEARING};
state_t state = START;
const int startupDelay = 10000; // 10 seconds

// Compass
LSM303 compass;
float heading;
float targetBearing = 0;  // Angle from the North

// Solenoid Valve
class Solenoid {
    Solenoid(uint8_t _pin){
      pin = _pin;
    void init(int _freq = 1){
      pinMode(pin, OUTPUT);
      freq = _freq;

    void enable() {
      enabled = true;

    void disable() {
      enabled = false;
      digitalWrite(pin, LOW);

    void run() {
      unsigned long currentMillis = millis();
      if(enabled && (currentMillis - lastMillis) > (1000/freq) ) {
        state = !state;   // Toggle the state
        digitalWrite(pin, state);
        lastMillis = currentMillis;

    uint8_t pin;    // Pin that controls the solenoid
    boolean state = LOW;    // Keep track of solenoid state
    boolean enabled = false;  // Flag to start and stop the motion
    int freq;
    unsigned long lastMillis = millis();  // Keep the last time
const uint8_t solenoidPin = 4;  // Pin that the mosfet is conected to
const int solenoidFreq = 1;     // Frequency of the solenoid movement
Solenoid solenoid(solenoidPin);

// Servo
Servo servo;
const uint8_t servoPin = 3;         // Pin that the servomotor is connected to
int servoStartingPos = 90;    // variable to store the servo starting position
const float swingAngle = 45; // Maximum steering angle to either side
void steer(float turn) {
  // Follow the shorter arc
  Serial.print("Original: "); Serial.print(turn);
  if(turn > 180){
    turn -= 360;
  else if(turn < -180){
    turn += 360;
  Serial.print("\tTurn: "); Serial.print(turn);
  // Compute the proportion of steering angle
  float steering = map(turn, -180, 180, -45, 45);   // (turn * swingAngle) /180.0;
  Serial.print("\tSteering by: "); Serial.println(steering);
  float position = servoStartingPos + steering;
  position = constrain(position, 45, 135);  // throttle the swing: between 45 and 135
  servo.write(position);  // tell servo to go to position in variable 'pos'
  delay(15);              // waits 15ms for the servo to reach the position  

// Reed Switch connected Wheel
class Wheel {
    Wheel(uint8_t _reed, float _dia) {
      reedSwitch = _reed;
      diameter = _dia;

    void init() {
      pinMode(reedSwitch, INPUT_PULLUP);
    void countHalfRev() {

    void resetCounter() {
      halfRevCounter = 0;
    // Distance moved in mm
    float getDistance() {
      // Compute distance from the number of revolutions
      float rev = halfRevCounter/(float)2;
      float perimeter = PI * diameter;
      return rev * perimeter;
    unsigned char reedSwitch; // Reed switch pin
    float diameter; // Wheel diameter in mm
    volatile unsigned long halfRevCounter = 0;    // Wheel half revolutions counter
const uint8_t reedSwitch = 2;   // Pin that the switch is conected to
const float wheelDiameter = 70;   // Wheel diameter in mm
Wheel wheel(reedSwitch, wheelDiameter);
// Reed switch interrupt handler
void reedSwitchInterrupt() {
  // each signal corresponds to a half revolution
const float thresholdDistance = 1000; // Distance in mm 
float dist; // Distancewheel  moved

void setup() {
  // Initialize serial
  // Initialize compass

  // Configure Servo
  servo.write(servoStartingPos);    // Position servo to center
  // Initialize solenoid

  // Initialize wheel
  // attach interrupt to capture reed switch signals
  attachInterrupt(digitalPinToInterrupt(reedSwitch), reedSwitchInterrupt, FALLING);

void loop() {;
  // Read compass;
  heading = compass.heading();
  // Run Finite State
  switch(state) {
    case START:
      // Enable solenoid
      state = STRIDE;

    case STRIDE:
      dist = wheel.getDistance();
      Serial.print("Distance (mm): "); Serial.print(dist);
      Serial.print("\tHeading (deg): "); Serial.println(heading);
      if(dist >= thresholdDistance) {
        // Reset the counter
        state = BEARING;

    case BEARING:
      Serial.print("Heading (deg): "); Serial.println(heading);
      // Turn servo
      steer(heading - targetBearing);


Perhaps you meant magnetometer, instead of accelerometer.

If so, be aware that magnetometers are extremely sensitive to magnetic fields in their vicinity, and such fields are created by motors, servos, solenoids and magnetized objects. If this is a problem with your robot, move the magnetometer away from sources of magnetic fields.

Yes sorry it is a magnetometer, i thought the words were interchangeable as my device was listed as an accelerometer. I will try moving further from the other components which could influence the magnetic field, my only concern is that it works very well with the testing code that I did not write, and it fluctuates pretty drastically with this code. Thanks for the reply!

The LSM303 contains both an accelerometer and a magnetometer. They are completely different sensors and together, can be used to make a tilt-compensated compass.

You didn't show any of the code that actually determines the heading, so if you want help with that, post that code and a quoted text example showing the problem.

Note that to be useful for a compass, magnetometers must be calibrated. Overview here.