Stepper_motor_controller firmware for LSM303DLHC sensor

Hi all. I’m making a homebrew rotator based on the next hardware elements:

  • Arduino Uno
  • CNC Shield
  • DRV8825
  • Stepper motors 17HD48002H-22B
  • LSM303DLHC sensor

The CNC Shield is configured with all jumpers to work with 1/32 microsteps. Next a picture with the LSM303DLHC sensor sticked in the arm (still in development):

I’ve modified the original stepper_motor_controller Arduino firmware to fit into my needs. I’m into trouble because the steppers don’t run to the desired position according to sensor readings. Next my modified code (not yet much debugged, sorry):
/*!
* @file stepper_motor_controller.ino
*
* This is the documentation for satnogs rotator controller firmware
* for stepper motors configuration. The board (PCB) is placed in
*
* satnogs-rotator-controller
and is for releases:
* v2.0
* v2.1
* v2.2
* wiki page
*
* @section dependencies Dependencies
*
* This firmware depends on
* AccelStepper library
being present on your system. Please make sure you
* have installed the latest version before using this firmware.
*
* @section license License
*
* Licensed under the GPLv3.
*
*/

#define SAMPLE_TIME        0.1      ///< Control loop in s
#define RATIO              3.4375   ///< Gear ratio of rotator gear box                                 default 54
#define MICROSTEP          32       ///< Set Microstep
#define MIN_PULSE_WIDTH    20       ///< In microsecond for AccelStepper
#define MAX_SPEED          500      ///< In steps/s, consider the microstep
#define MAX_ACCELERATION   800      ///< In steps/s^2, consider the microstep
#define SPR                6400     ///< Step Per Revolution, consider the microstep
#define MIN_M1_ANGLE       0        ///< Minimum angle of azimuth
#define MAX_M1_ANGLE       360      ///< Maximum angle of azimuth
#define MIN_M2_ANGLE       0        ///< Minimum angle of elevation
#define MAX_M2_ANGLE       90       ///< Maximum angle of elevation
#define DEFAULT_HOME_STATE LOW      ///< Change to LOW according to Home sensor
#define HOME_DELAY         12000    ///< Time for homing Deceleration in millisecond

#include <AccelStepper.h>
#include <Wire.h>
#include <globals.h>
#include <easycomm.h>
#include <rotator_pins.h>
#include <endstop.h>
#include <PID_v1.h>
#include <MultiStepper.h>

// Includes for sensor libraries
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_9DOF.h>

uint32_t t_run = 0; // run time of uC
easycomm comm;
AccelStepper stepper_az(AccelStepper::DRIVER, M1IN1, M1IN2);
AccelStepper stepper_el(AccelStepper::DRIVER, M2IN1, M2IN2);

enum _rotator_error homing();
int32_t deg2step(float deg);
float step2deg(int32_t step);

// Initial assignmets to sensors
/* Assign a unique ID to the sensors */
Adafruit_9DOF                dof   = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
/* Update this with the correct SLP for accurate altitude measurements */
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

// Routine declarations for sensors (at the end)
void initSensors();
float getPitch(void);
float getHeading(void);

void setup() {
    
    // Serial Communication
    comm.easycomm_init();
    Serial.begin(115200);

    // Stepper Motor setup
    stepper_az.setEnablePin(MOTOR_EN);
    stepper_az.setPinsInverted(false, false, true);
    stepper_az.enableOutputs();      
    stepper_az.setMaxSpeed(MAX_SPEED);
    stepper_az.setAcceleration(MAX_ACCELERATION);
    stepper_az.setMinPulseWidth(MIN_PULSE_WIDTH);

    stepper_el.setEnablePin(MOTOR_EN);
    stepper_el.setPinsInverted(false, false, true);
    stepper_el.enableOutputs();    
    stepper_el.setMaxSpeed(MAX_SPEED);
    stepper_el.setAcceleration(MAX_ACCELERATION);
    stepper_el.setMinPulseWidth(MIN_PULSE_WIDTH);

    /* Initialise the sensors */
    initSensors();   
   
}

void loop() {
    
    // Run easycomm implementation
    comm.easycomm_proc();

    // Get position of both axis
    control_az.input = step2deg(stepper_az.currentPosition());
    control_el.input = step2deg(stepper_el.currentPosition());    

    // Check rotator status
    if (rotator.rotator_status != error) {
        if (rotator.homing_flag == false) {            
            // Check home flag
            rotator.control_mode = position;
            // Homing
            // rotator.rotator_error = homing(deg2step(-MAX_M1_ANGLE), deg2step(-MAX_M2_ANGLE));
            //rotator.rotator_error = homing(deg2step(getHeading()), deg2step(getPitch()));
            //rotator.rotator_error = homing(deg2step(MIN_M1_ANGLE - 1), deg2step(MIN_M2_ANGLE - 1));
            rotator.rotator_error = homing();
            
            if (rotator.rotator_error == no_error) {
                // No error
                rotator.rotator_status = idle;
                rotator.homing_flag = true;
            } else {
                // Error
                rotator.rotator_status = error;
                rotator.rotator_error = homing_error;
            }
        } else {             
            // Control Loop
            stepper_az.moveTo(deg2step(control_az.setpoint));
            stepper_el.moveTo(deg2step(control_el.setpoint));
            rotator.rotator_status = pointing;
            // Move azimuth and elevation motors            
            stepper_az.run();
            stepper_el.run();            
            // Idle rotator
            if (stepper_az.distanceToGo() == 0 && stepper_el.distanceToGo() == 0) {
                rotator.rotator_status = idle;
            }
        }
    } else {
        // Error handler, stop motors and disable the motor driver
        stepper_az.stop();
        stepper_az.disableOutputs();
        stepper_el.stop();
        stepper_el.disableOutputs();
        if (rotator.rotator_error != homing_error) {
            // Reset error according to error value
            rotator.rotator_error = no_error;
            rotator.rotator_status = idle;
        }
    }
}

/**************************************************************************/
/*!
    @brief    Move both axis with one direction in order to find home position,
              end-stop switches
    @param    seek_az
              Steps to find home position for azimuth axis
    @param    seek_el
              Steps to find home position for elevation axis
    @return   _rotator_error
*/
/**************************************************************************/

// NEW HOMING ROUTINE TAKE INTO ACCOUNT THE SENSORS
enum _rotator_error homing() {
  
    float az_actual = getHeading();
    float el_actual = getPitch();
    
    // Set the steppers position with sensor reading
    stepper_az.setCurrentPosition(deg2step(az_actual));
    stepper_el.setCurrentPosition(deg2step(el_actual));
    
    // Moving steppers to 0
    stepper_az.setSpeed(MAX_SPEED); stepper_el.setSpeed(MAX_SPEED);
    stepper_az.runToNewPosition(0); stepper_el.runToNewPosition(0);
    
    // Delay to Deccelerate and homing, to complete the movements
    uint32_t time = millis();
    while (millis() - time < HOME_DELAY) {
       // wdt.watchdog_reset();        
        stepper_az.run();
        stepper_el.run();        
    }
    
    // Set the home position and reset all critical control variables
    stepper_az.setCurrentPosition(0);
    stepper_el.setCurrentPosition(0);
    control_az.setpoint = 0;
    control_el.setpoint = 0;

    return no_error;  
}


/**************************************************************************/
/*!
    @brief    Convert degrees to steps according to step/revolution, rotator
              gear box ratio and microstep
    @param    deg
              Degrees in float format
    @return   Steps for stepper motor driver, int32_t
*/
/**************************************************************************/
int32_t deg2step(float deg) {
    return (RATIO * SPR * deg / 360);
}

/**************************************************************************/
/*!
    @brief    Convert steps to degrees according to step/revolution, rotator
              gear box ratio and microstep
    @param    step
              Steps in int32_t format
    @return   Degrees in float format
*/
/**************************************************************************/
float step2deg(int32_t step) {
    return (360.00 * step / (SPR * RATIO));
}

/*
 * -- Sensors routines
 */

// -- Init sensor routine
void initSensors()
{
  if(!accel.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while(1);
  }
  if(!mag.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
}

// -- Routines for getting orientation.
// -- Pitch (X) is between -180º and +180º and heading (Z) between 0º and +359º
float getPitch(void)
{
  sensors_event_t accel_event; 
  // sensors_event_t mag_event; 
  sensors_vec_t   orientation;

  /* Calculate pitch and roll from the raw accelerometer data */
  accel.getEvent(&accel_event);
  if (dof.accelGetOrientation(&accel_event, &orientation))
  {
    /* 'orientation' should have valid .roll and .pitch fields */
    // Serial.print(F("Roll: "));
    // Serial.print(orientation.roll);
    // Serial.print(F("; "));
    /*Serial.print(F("Pitch (elevación): "));
    Serial.print(orientation.pitch);
    Serial.println(F("; "));*/
  }

  return orientation.pitch;  
}

float getHeading(void)
{
  // sensors_event_t accel_event;
  sensors_event_t mag_event;
  sensors_vec_t   orientation;
  
  /* Calculate the heading using the magnetometer */
  mag.getEvent(&mag_event);
  if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation))
  {
    /* 'orientation' should have valid .heading data now */
    /*Serial.print(F("Heading (azimut): "));
    Serial.print(orientation.heading);
    Serial.print(F("; "));*/
  } 
    
  return orientation.heading;
}

Has anyone experience with the same hardware than me?

Thanks in advance.

A diff might be easier to review.