Lego Robotics with Arduino 1

I have a bunch of these little motors recovered from broken printers. They have a neat feedback system built onto the back using a slotted infrared sensor and an encoding wheel. All that needs to be added is a 74LS14 (or equivalent) Schmitt trigger to clean up the pulses coming from the infrared sensor.

I modified the motor spindle to take the smallest Lego Technic gear and built a little test rig with a linear actuator type arrangement. The motors can be glued upside-down to a 4x2 stud Lego plate and occupy roughly the same space as a classic Lego Technic 4.5 volt motor.

VIDEO - Lego Robotics Test Rig

The motor is driven by a L293NE H-bridge IC with another of the Schmitt inverters used to reduce pin count to just 2 - MotorDirection and MotorSpeed. The code uses an interrupt to count the incoming pulses on a pin and also detects a stall situation by checking if the pulse count has stopped when the motor is supposed to be running.

TEST CODE

// Lego Robotics Test 002

// Glenn Walsh 03/10/2022

// Testing IR slot position feedback and limit detection

volatile int pulseCount = 0;

volatile int lastPulseCount = 0;

int maxCount = 0;

const int speedPin = 9;       // PWM speed pin (0-255)

const int directionPin = 8;   // Direction pin 1/0

bool motorDirection = 0;

bool motorRun = 0;

long lastChangeTime = 0;

void setup() {

  // SETUP pins

  Serial.begin(9600);

  pinMode(speedPin, OUTPUT);      // PWM speed pin

  pinMode(directionPin, OUTPUT);  // Direction pin 1

  stopMotor() ;                                    // Set speed to stop

  attachInterrupt(0, pulseCounter, FALLING);

  getMaxPulses();

  Serial.println(maxCount);

}

void loop() {

  // TEST - goto one end and then come the other way

  // 30 pulses at a time until close to maxCount value

  gotoEndPosition(1);

  pulseCount = 0;

  digitalWrite(directionPin, 0);

  while (pulseCount < (maxCount - 30)) {

    startMotor(255);

    lastPulseCount = pulseCount;

    while (pulseCount - lastPulseCount < 20) {

    // WAIT

    }

    stopMotor();

    newDelay(1000);

  }

}

void pulseCounter() {

  pulseCount++;

}

void getMaxPulses () {

  // Start by moving to one end in case actuator is in the middle

  // Ignore pulses this time

  gotoEndPosition (1);

  // Run motor each way and take average number of pulses

  gotoEndPosition (0);

  maxCount = pulseCount; // Get average count

  pulseCount = 0;

  gotoEndPosition (1);

  maxCount = (pulseCount + maxCount) / 2; // Get average count

  pulseCount = 0;

}

void gotoEndPosition(bool motorEnd) {

  // Move to end point count pulses and stop

  digitalWrite(directionPin, motorEnd);

  startMotor(255);

  while (motorRun) {

    lastPulseCount = pulseCount;

    newDelay(100);

    if (pulseCount - lastPulseCount < 25) {

      // STALL!

      // Not enough pulses received in 100ms so motor has stalled

      stopMotor();

    }

  }

}

void stopMotor() {

  // Stop the motor

  analogWrite(speedPin, 0);

  motorRun = false;

}

void startMotor(int speedValue) {

  // Start the motor with at given speed (0-255)

  analogWrite(speedPin, speedValue);

  motorRun = true;

}

void newDelay(int period) {

  // Non-blocking short delay to allow interrupt to work as expected

  lastChangeTime = millis();

  while (millis() <= lastChangeTime + period) {

    // WAIT!

  }

}