Help with ToF sensors controlling stepper motor

Other Hardware Sensors
1

Hi all,

I've been working on a project to allow my observatory dome to track my telescopes movements using ToF sensors. Basically if a sensor is blocked, the motor moves the dome until it is unblocked and if it is blocked for a set amount of time, the motor speeds up to catch up.

However, when I've tested the code, the motor moves in loads of tiny increments instead of a single smooth move. Other test code without the sensors works perfectly fine so the wiring is ok. I think its the sensors that are causing the issue.

I am using 4 Adafruit VL53LX1's connected to an Adafruit PCA9548 connected to a Leonardo board.

Does anyone have any ideas on the issue?

Thank you


#include <Wire.h>
#include "Adafruit_VL53L1X.h"

#define TCAADDR 0x70
void tcaselect(uint8_t i) {
  if (i > 7) return;
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();
}

Adafruit_VL53L1X vl[4];  
bool sensorOK[4] = {false, false, false, false};
#define SENSOR_THRESHOLD_MM 200 

#define DIR_PIN 5
#define PUL_PIN 6

bool motorRunning = false;
bool motorDirCW = true;
uint32_t currentDelay = 1500;
unsigned long lastStepTime = 0;

#define START_DELAY 2000    
#define FAST_DELAY  800     
#define ACCEL_TIME  5000000 
unsigned long blockStart = 0;

void setup() {
  pinMode(DIR_PIN, OUTPUT);
  pinMode(PUL_PIN, OUTPUT);
  digitalWrite(DIR_PIN, LOW);
  digitalWrite(PUL_PIN, LOW);

  Serial.begin(115200);
  delay(1000);
  Serial.println("=== Observatory Dome Automation ===");

  Wire.begin();

  for (int i = 0; i < 4; i++) {
    tcaselect(i);
    if (!vl[i].begin(0x29, &Wire)) {
      Serial.print("Sensor "); Serial.print(i); Serial.println(" FAIL");
      sensorOK[i] = false;
    } else {
      vl[i].startRanging();
      sensorOK[i] = true;
      Serial.print("Sensor "); Serial.print(i); Serial.println(" OK");
    }
  }

  Serial.println("Commands: CW, CCW, STOP, SPDxxxx, TEST");
}

void loop() {

  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    handleSerialCommand(cmd);
  }

  if (manualMode()) {
    runMotor();
    return;
  }

  bool leftBlocked = false;
  bool rightBlocked = false;

  for (int i = 0; i < 4; i++) {
    if (!sensorOK[i]) continue;
    tcaselect(i);
    if (vl[i].dataReady()) {
      int dist = vl[i].distance();
      vl[i].clearInterrupt();
      if (dist > 0 && dist < SENSOR_THRESHOLD_MM) {
        if (i < 2) leftBlocked = true;  
        else rightBlocked = true;       
      }
    }
  }

  if (leftBlocked && !rightBlocked) {
    startMotor(true);
  } else if (rightBlocked && !leftBlocked) {
    startMotor(false);
  } else if (!leftBlocked && !rightBlocked) {
    stopMotor();
  }

  if (motorRunning) {
    if (blockStart == 0) blockStart = micros();
    unsigned long elapsed = micros() - blockStart;
    if (elapsed > ACCEL_TIME) {
      currentDelay = FAST_DELAY;
    } else {
      currentDelay = START_DELAY - ( (START_DELAY - FAST_DELAY) * elapsed / ACCEL_TIME );
    }
  } else {
    blockStart = 0;
  }

  runMotor();
}

bool manualModeFlag = false;
bool manualMode() {
  return manualModeFlag;
}

void runMotor() {
  if (motorRunning) {
    unsigned long now = micros();
    if (now - lastStepTime >= currentDelay) {
      lastStepTime = now;
      digitalWrite(PUL_PIN, HIGH);
      delayMicroseconds(2);
      digitalWrite(PUL_PIN, LOW);
    }
  }
}

void startMotor(bool cw) {
  motorRunning = true;
  manualModeFlag = false;
  digitalWrite(DIR_PIN, cw ? HIGH : LOW);
}

void stopMotor() {
  motorRunning = false;
}

void handleSerialCommand(String cmd) {
  cmd.trim();
  cmd.toUpperCase();

  if (cmd == "CW") {
    Serial.println("Manual CW");
    manualModeFlag = true;
    motorRunning = true;
    digitalWrite(DIR_PIN, HIGH);

  } else if (cmd == "CCW") {
    Serial.println("Manual CCW");
    manualModeFlag = true;
    motorRunning = true;
    digitalWrite(DIR_PIN, LOW);

  } else if (cmd == "STOP") {
    Serial.println("STOP");
    motorRunning = false;

  } else if (cmd.startsWith("SPD")) {
    long spd = cmd.substring(3).toInt();
    if (spd >= 100) {
      currentDelay = spd;
      Serial.print("Manual speed: ");
      Serial.println(currentDelay);
    } else {
      Serial.println("Invalid SPD value");
    }

  } else if (cmd == "TEST") {
    Serial.println("Test: 200 steps CW then CCW");
    digitalWrite(DIR_PIN, HIGH);
    for (int i = 0; i < 200; i++) {
      digitalWrite(PUL_PIN, HIGH);
      delayMicroseconds(800);
      digitalWrite(PUL_PIN, LOW);
      delayMicroseconds(800);
    }
    delay(500);
    digitalWrite(DIR_PIN, LOW);
    for (int i = 0; i < 200; i++) {
      digitalWrite(PUL_PIN, HIGH);
      delayMicroseconds(800);
      digitalWrite(PUL_PIN, LOW);
      delayMicroseconds(800);
    }
    Serial.println("Test complete.");

  } else {
    Serial.print("Unknown: ");
    Serial.println(cmd);
  }
}
2

What, exactly, does blocking as TOF sensor mean? They are meant to measure distance and have a minimum and a maximum distance they can report. A data sheet/information on the sensor in question might be useful.

3

As a former astronomer and current electronics guy, I think the ToF sensor is the worst choice.
I can't recommend something better without knowing what the 'block' is.
Why are you not simply making the dome motor move at the same rate as your mount motor. There are likely a couple ways to accomplish that.

4

Blocking in my application is if there is an obstruction detected at a distance of 200mm or less (observatory dome slit edge) then the sensor reads as blocked, anything over that is seen as clear (looking into sky, no obstruction).

Here is the sensor documentation:

5

Put in serial.print statements to see what the sensors are reporting, and what actions are being taken at all times. Match those reports with your expectations for the reported conditions.

6

That should be in your FIRST posting in the thread!

8

I would have complete confidence in ASCOM, but if that is your decision, then use something like IR transmitter/receiver sensors. ToF is not a good solution for your situation. Good luck.

9

Can you make a drawing with dimensions of one of the sensor/reflector units showing the attachment locations. How large is the target for the laser? Isn’t one piece moving on a curved path? Seems like geometry would cause failures.