robotics/motors-viam.js

Robotics.dev P2P ROS2 robot controller CLI with ROS telemetry and video streaming

import { createRequire } from "module"; const require = createRequire(import.meta.url); import Configstore from "configstore"; const config = new Configstore("robotics"); var robotId = config.get("ROBOT_ID"); var apiToken = config.get("API_TOKEN"); const gpio = require("@iiot2k/gpiox"); const rclnodejs = require("rclnodejs"); const math = require("mathjs");

// Checks for --pins and if it has a value
const pinsIndex = process.argv.indexOf('--pins');
let pinsValue;
let pins;
if (pinsIndex > -1) {
  pinsValue = process.argv[pinsIndex + 1];
  pins = (pinsValue.split(","))
} else {
  pins = "27,22,17,18".split(",");
}
console.log('Pins:', `${pins}`);

// Motor pin configuration
const LEFT_MOTOR_PIN1 = pins[0];
const LEFT_MOTOR_PIN2 = pins[1];
const RIGHT_MOTOR_PIN1 = pins[2];
const RIGHT_MOTOR_PIN2 = pins[3];

// Encoder pin configuration
const LEFT_ENCODER_GPIO = 37;
const RIGHT_ENCODER_GPIO = 35;

// Robot configuration
const TICKS_PER_REVOLUTION = 1992 * 2;
const DISTANCE_BETWEEN_WHEELS_M = 0.26;
const WHEEL_CIRCUMFERENCE_MM = 217;
const MAX_SPEED = 0.3;
const MAX_PWM = 100;

// Motor control functions
function setMotorSpeed(pin1, pin2, speed) {
    if (speed > 0) {
        gpio.pwm_gpio(pin1, 200, speed);
        gpio.pwm_gpio(pin2, 200, 0);
    } else {
        gpio.pwm_gpio(pin1, 200, 0);
        gpio.pwm_gpio(pin2, 200, Math.abs(speed));
    }
}

function stopMotors() {
    gpio.pwm_gpio(LEFT_MOTOR_PIN1, 200, 0);
    gpio.pwm_gpio(LEFT_MOTOR_PIN2, 200, 0);
    gpio.pwm_gpio(RIGHT_MOTOR_PIN1, 200, 0);
    gpio.pwm_gpio(RIGHT_MOTOR_PIN2, 200, 0);
}

// Initialize GPIO pins
gpio.init_gpio(LEFT_MOTOR_PIN1, gpio.GPIO_MODE_OUTPUT, 0);
gpio.init_gpio(LEFT_MOTOR_PIN2, gpio.GPIO_MODE_OUTPUT, 0);
gpio.init_gpio(RIGHT_MOTOR_PIN1, gpio.GPIO_MODE_OUTPUT, 0);
gpio.init_gpio(RIGHT_MOTOR_PIN2, gpio.GPIO_MODE_OUTPUT, 0);

// ROS2 Node
async function main() {
    await rclnodejs.init();
    const node = new rclnodejs.Node('motor_controller');

    // Subscribe to cmd_vel topic
    node.createSubscription('geometry_msgs/msg/Twist', `robot${robotId.replace(/-/g, "")}/cmd_vel`, (msg) => {
        const linear_x = -msg.linear.x;  // Forward/backward motion (inverted)
        const angular_z = msg.angular.z; // Rotation

        // Convert twist to motor speeds (range -1 to 1)
        let leftSpeed = linear_x - angular_z;
        let rightSpeed = linear_x + angular_z;

        // Clamp values between -1 and 1
        leftSpeed = Math.max(-1, Math.min(1, leftSpeed));
        rightSpeed = Math.max(-1, Math.min(1, rightSpeed));

        // Set motor speeds
        setMotorSpeed(LEFT_MOTOR_PIN1, LEFT_MOTOR_PIN2, leftSpeed * 100);
        setMotorSpeed(RIGHT_MOTOR_PIN1, RIGHT_MOTOR_PIN2, rightSpeed * 100);
    });

    // Handle shutdown
    process.on('SIGINT', () => {
        console.log('Shutting down...');
        stopMotors();
        node.destroy();
        process.exit(0);
    });

    node.spin();
}

main().catch(console.error);