Controlling LIDAR, Motor and Encoder

Other Hardware Robotics
1

Hi all, for this project i am building a sort of 'radar' using tfmini lidar that spins using a nema 17 motor and rotary encoder for positional data.

I am using an arduino Nano and am using a pin change interrupt for the encoder which works fine. With regard to the lidar and motor, i want a fast sampling rate for the lidar and a steady constant velocity for the motor.

My first approach was to use the AccelStepper library and have this in the motor function

void loop() 
{
  Lidar();
  motor ();
}

void motor ()
{
  stepper.setSpeed(500);
  stepper.run();
}


void Lidar()
{
  uint16_t dist = tfmini.getDistance();
  uint16_t strength = tfmini.getRecentSignalStrength();
  Serial.println(dist);
  Serial.print(" cm      sigstr: ");
  Serial.println(strength);
}

However thus results in a fast sample rate for the Lidar but a 'jerky' and slow response from the motor.

I then used a different approach not using the library

void loop() 
{
  Lidar();
  motor ();
}

void motor ()
{
  digitalWrite(dirPin,HIGH); 
  for(int x = 0; x < 200; x++) {
    digitalWrite(stepPin,HIGH); 
    delayMicroseconds(2000); 
    digitalWrite(stepPin,LOW); 
    delayMicroseconds(2000); 
  }
}


void Lidar()
{
  uint16_t dist = tfmini.getDistance();
  uint16_t strength = tfmini.getRecentSignalStrength();
  Serial.println(dist);
  Serial.print(" cm      sigstr: ");
  Serial.println(strength);
}

This results in a good steady motor speed but due to the 'delay' the Lidar sample rate is slow.

My next thoughts were to
-use a second interrupt on the lidar but this has had no succes thus far
-research into arduino RTOS
-use a second microcontroller for the motor functionality

Unless of course you had any other ideas

My full code can be seen here

//Include Libraries
#include <Stepper.h>
#include <EEPROM.h>
#include <SoftwareSerial.h>
#include "TFMini.h"
#include <AccelStepper.h>

//Definitions Motor
#define dirPin 4
#define stepPin 5
#define motorInterfaceType 1
int speed = 500;

AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

// Setup Serial Port LIDAR
SoftwareSerial mySerial(12, 13); 
TFMini tfmini; // Uno RX (TFMINI TX), Board TX (TFMINI RX)

//Definition Encoder
int pinA = 2; 
int pinB = 3; 
int encoderPosCount = 0; //EEPROM.read(encoderPosCount);
int encoderInterrupt = 0;
int pinALast;  
int aVal = 0;
int address = 0;

void setup() 
{
  
  //Motor Setup
  stepper.setMaxSpeed(1000);

  //Encoder Setup
  pinMode (pinA,INPUT);
  pinMode (pinB,INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(pinB), rotation, CHANGE);
  
  pinALast = digitalRead(pinA); 
  Serial.begin(38400);

  // Wait for Serial Port to Connect
  while (!Serial);
  Serial.println ("Serial Port Found");
  mySerial.begin(TFMINI_BAUDRATE);
  tfmini.begin(&mySerial);
  
}

void loop() 
{
  Lidar();
  motor ();
}

void motor ()
{
  digitalWrite(dirPin,HIGH); 
  for(int x = 0; x < 200; x++) {
    digitalWrite(stepPin,HIGH); 
    delayMicroseconds(2000); 
    digitalWrite(stepPin,LOW); 
    delayMicroseconds(2000); 
  }
}


void Lidar()
{
  uint16_t dist = tfmini.getDistance();
  uint16_t strength = tfmini.getRecentSignalStrength();
  Serial.println(dist);
  Serial.print(" cm      sigstr: ");
  Serial.println(strength);
}

void rotation()
{
  aVal = digitalRead(pinA);
  
  if (aVal != pinALast)
   { 
     if (digitalRead(pinB) != aVal) //We're Rotating Clockwise
     { 
       encoderPosCount --;
       Serial.println ("Rotate Clockwise");
       encoderInterrupt = encoderInterrupt + 1;
     } 
     
     else if(encoderPosCount == 118)
     {
      encoderPosCount = 0;
     }
     
     else 
     {
       Serial.println("Rotate Counterclockwise");
       encoderPosCount++;
       encoderInterrupt = encoderInterrupt - 1;
     }
   }
     
     Serial.print("Encoder Count: ");
     Serial.println(encoderPosCount);
     EEPROM.write(address, encoderPosCount);
     Serial.println();
   
   pinALast = aVal;
}

Thanks