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.
