robofoundry
commented
2 years ago I figured out what was wrong. My values here were wrong, hopefully it helps someone else.
define COUNTS_PER_REV1 339 // wheel1 encoder's no of ticks per rev
define COUNTS_PER_REV2 346 // wheel2 encoder's no of ticks per rev
define COUNTS_PER_REV3 0 // wheel3 encoder's no of ticks per rev
define COUNTS_PER_REV4 0 // wheel4 encoder's no of ticks per rev
changed them to :
define COUNTS_PER_REV1 26275 // wheel1 encoder's no of ticks per rev
define COUNTS_PER_REV2 23685 // wheel2 encoder's no of ticks per rev
define COUNTS_PER_REV3 0 // wheel3 encoder's no of ticks per rev
define COUNTS_PER_REV4 0 // wheel4 encoder's no of ticks per rev
and things started behaving much better. There is still a lot of overshoot with a bunch of buffered messages that play out before the motor stops after letting go of the joy stick but atleast it stops.
Also, the blinking with LED is apparently normal and it comes from this function: void twistCallback(const void * msgin) { digitalWrite(LED_PIN, !digitalRead(LED_PIN)); <-- This line is toggling the LED as it receives the messages
prev_cmd_time = millis();}
Hello, First of all thank you for such a wonderful project with so many thoughtful things that are added to the project including the great documentation and build instructions. I was able to build a robot with 2wd differential drive robot with Teensy 4.1. Everything went mostly smoothly and I was able to run the calibration with sample command and adjust all the motors encoders.
However, after trying to bringup the robot very first time and then running it either via teleop keyboard or teleop joystick, as soon as the launch file is executed, it starts flashing the LED on Teensy 4.1 continuously. Also, first time I enter any command it seems to run the motors in one direction and then motors just run continuously until I kill the process.
I tried to troubleshoot by commenting out big chunks of code in main .ino file and it seems to be having some trouble as soon as the subscriber node is added in following lines of code and after that it never recovers from the error. I tried to follow micro ros examples from repo here [https://github.com/micro-ROS/micro_ros_arduino] and it seems to do the same thing for a very simple example of publisher/subscriber, as soon as I add a subscriber, it starts flashing the LED from errorLoop.
executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&executor, &support.context, 2, & allocator)); RCCHECK(rclc_executor_add_subscription( &executor, &twist_subscriber, &twist_msg, &twistCallback, ON_NEW_DATA ));
My system specs are as follows: Linux Focal 20.04 Main computer with ROS2 Foxy Raspberry Pi 4 for robot computer Teensy 4.1 for micro-controller 2-GoBuilda DC motors 2- L298N motor controllers, one for each motor I don't even have lidar or camera mounted on robot yet, just the IMU and motors. The header file constants are as follows, please let me know if you have any troubleshooting ideas that I can try or any debugging code I can add that will allow me to narrow down the issue. Thanks.
lino_base_config.h constants below:
//uncomment the base you're building
define LINO_BASE DIFFERENTIAL_DRIVE // 2WD and Tracked robot w/ 2 motors
// #define LINO_BASE SKID_STEER // 4WD robot // #define LINO_BASE MECANUM // Mecanum drive robot
//uncomment the motor driver you're using
define USE_GENERIC_2_IN_MOTOR_DRIVER // Motor drivers with 2 Direction Pins(INA, INB) and 1 PWM(ENABLE) pin ie. L298, L293, VNH5019
// #define USE_GENERIC_1_IN_MOTOR_DRIVER // Motor drivers with 1 Direction Pin(INA) and 1 PWM(ENABLE) pin. // #define USE_BTS7960_MOTOR_DRIVER // BTS7970 Motor Driver // #define USE_ESC_MOTOR_DRIVER // Motor ESC for brushless motors
//uncomment the IMU you're using // #define USE_GY85_IMU // #define USE_MPU6050_IMU // #define USE_MPU9150_IMU
define USE_MPU9250_IMU
define K_P 0.6 // P constant
define K_I 0.8 // I constant
define K_D 0.5 // D constant
/ ROBOT ORIENTATION FRONT MOTOR1 MOTOR2 (2WD/ACKERMANN) MOTOR3 MOTOR4 (4WD/MECANUM)
BACK /
//define your robot' specs here
define MOTOR_MAX_RPM 435 // motor's max RPM
define MAX_RPM_RATIO 0.80 // max RPM allowed for each MAX_RPM_ALLOWED = MOTOR_MAX_RPM * MAX_RPM_RATIO
define MOTOR_OPERATING_VOLTAGE 12 // motor's operating voltage (used to calculate max RPM)
define MOTOR_POWER_MAX_VOLTAGE 13 // max voltage of the motor's power source (used to calculate max RPM)
define MOTOR_POWER_MEASURED_VOLTAGE 12.87 // current voltage reading of the power connected to the motor (used for calibration)
define COUNTS_PER_REV1 339 // wheel1 encoder's no of ticks per rev
define COUNTS_PER_REV2 346 // wheel2 encoder's no of ticks per rev
define COUNTS_PER_REV3 0 // wheel3 encoder's no of ticks per rev
define COUNTS_PER_REV4 0 // wheel4 encoder's no of ticks per rev
define WHEEL_DIAMETER 0.1 // wheel's diameter in meters
define LR_WHEELS_DISTANCE 0.38 // distance between left and right wheels
define PWM_BITS 10 // PWM Resolution of the microcontroller
define PWM_FREQUENCY 20000 // PWM Frequency
// INVERT ENCODER COUNTS
define MOTOR1_ENCODER_INV false
define MOTOR2_ENCODER_INV true
define MOTOR3_ENCODER_INV false
define MOTOR4_ENCODER_INV false
// INVERT MOTOR DIRECTIONS
define MOTOR1_INV false
define MOTOR2_INV false
define MOTOR3_INV false
define MOTOR4_INV false
// ENCODER PINS
define MOTOR1_ENCODER_A 14
define MOTOR1_ENCODER_B 15
define MOTOR2_ENCODER_A 11
define MOTOR2_ENCODER_B 12
define MOTOR3_ENCODER_A 17
define MOTOR3_ENCODER_B 16
define MOTOR4_ENCODER_A 9
define MOTOR4_ENCODER_B 10
// MOTOR PINS
ifdef USE_GENERIC_2_IN_MOTOR_DRIVER
define MOTOR1_PWM 22 //Pin no 21 is not a PWM pin on Teensy 4.x, you can swap it with pin no 1 instead.
define MOTOR1_IN_A 20
define MOTOR1_IN_B 1
define MOTOR2_PWM 5
define MOTOR2_IN_A 6
define MOTOR2_IN_B 8
define MOTOR3_PWM 24
define MOTOR3_IN_A 23
define MOTOR3_IN_B 0
define MOTOR4_PWM 4
define MOTOR4_IN_A 3
define MOTOR4_IN_B 2
define PWM_MAX pow(2, PWM_BITS) - 1
define PWM_MIN -PWM_MAX
endif