Forum

UART disconnecting ...
 
Notifications
Clear all

UART disconnecting issue

5 Posts
2 Users
0 Likes
113 Views
Forum 1
(@lgroth)
Active Member
Joined: 1 month ago
Posts: 3
Topic starter  

Hi,

we have 4 SOLO Pico Motorcontrollers connected to a Teensy 4.0 via UART/Serial. Sometimes they all work, but then occasionally one or more of them disconnect. These disconnects happen, seemingly at random, at first initialisation of later during operation. After reconnecting, the respective controller works for a short time before disconnecting again. I'll attach the Teensy code below, maybe we are missing something? 

We would appreciate some help regarding this,

Best Regards!

 

#include <PWMServo.h>
#include <ros.h>
#include "SOLOMotorControllersUart.h"
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <std_msgs/Float32MultiArray.h>

//-------------
// IMU SETUP
//-------------

Adafruit_BNO055 IMU = Adafruit_BNO055(55, 0x28, &Wire);

//-------------
// ROS SETUP
//-------------

ros::NodeHandle  nh;

// outputs to ROS
float yaw_angle = 0.0;
float torque_fb_fr = 0.0;
float torque_fb_fl = 0.0;
float torque_fb_rl = 0.0;
float torque_fb_rr = 0.0;
float rpm_fb_fr = 0.0;
float rpm_fb_fl = 0.0;
float rpm_fb_rl = 0.0;
float rpm_fb_rr = 0.0;
float battery_voltage = 10.0;
float car_data_array[10];
std_msgs::Float32MultiArray out_msg;
ros::Publisher car_data_pub("/car_data", &out_msg);

// inputs from ROS
float servo_angle_front = 90.0;
float servo_angle_rear = 90.0;
float torque_request_fl = 0.0;
float torque_request_fr = 0.0;
float torque_request_rl = 0.0;
float torque_request_rr = 0.0;
std_msgs::Float32MultiArray in_msg;
void cmdCb( const std_msgs::Float32MultiArray &in_msg){
  servo_angle_front = in_msg.data[0];
  servo_angle_rear = in_msg.data[1];
  torque_request_fl = in_msg.data[2];
  torque_request_fr = in_msg.data[3];
  torque_request_rl = in_msg.data[4];
  torque_request_rr = in_msg.data[5];
}
ros::Subscriber<std_msgs::Float32MultiArray> sub("/ros_data", cmdCb );


//-------------
// MOTOR SETUP
//-------------

/*wheel 1 to front 3 4 back
SOLO Obj1 => Wheel 1 ->CW FORWARD
SOLO Obj2 => Wheel 3 ->CW FORWARD
SOLO Obj4 => Wheel 2 ->CCW FORWARD
SOLO Obj3 => Wheel 4 ->CCW FORWARD
*/

// instanciate a SOLO object:
SOLOMotorControllers *MOTOR_FL;
SOLOMotorControllers *MOTOR_FR;
SOLOMotorControllers *MOTOR_RL;
SOLOMotorControllers *MOTOR_RR;

//-------------
// SERVO SETUP
//-------------
PWMServo front_servo;
PWMServo rear_servo;


void setup()
{
  tone(11, 262, 300);
  // IMU INITIALIZATION
  if (!IMU.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    //Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
  }

  tone(11, 262, 300);

  // ROS INITIALIZATION
  nh.initNode();
  nh.advertise(car_data_pub);
  nh.subscribe(sub);

  // 1
  tone(11, 262, 300);

  //SERVO INITIALIZATION
  front_servo.attach(9);
  rear_servo.attach(10);

  // 2
  tone(11, 262, 300);

  // MOTOR INITIALIZATION
  // Low Speed Low Performance Baudrate
  // Use this baudrate only for devices that don't support
  // 937500 or 921600 baudrates.
  
  Serial2.begin(937500);
  Serial3.begin(937500);
  Serial4.begin(937500);
  Serial5.begin(937500);

  int SOLOdeviceAddress = 100;
  MOTOR_FL = new SOLOMotorControllersUart(SOLOdeviceAddress, Serial3, SOLOMotorControllers::UartBaudrate::RATE_937500,1,1);
  MOTOR_FR = new SOLOMotorControllersUart(SOLOdeviceAddress, Serial5, SOLOMotorControllers::UartBaudrate::RATE_937500,1,1);
  MOTOR_RL = new SOLOMotorControllersUart(SOLOdeviceAddress, Serial2, SOLOMotorControllers::UartBaudrate::RATE_937500,1,1);
  MOTOR_RR = new SOLOMotorControllersUart(SOLOdeviceAddress, Serial4, SOLOMotorControllers::UartBaudrate::RATE_937500,1,1);

  delay(2000);

  // 3
  tone(11, 262, 300);

  int timeout = 0;
  while (MOTOR_FL->CommunicationIsWorking() == false)
  {
    //if( (timeout+=1) > 10 ) break;
    delay(50);
  }

  // 4
  tone(11, 262, 300);

  timeout = 0;
  while (MOTOR_FR->CommunicationIsWorking() == false)
  {
    //if( (timeout+=1) > 10 ) break;
    delay(50);
  }

  // 5
  tone(11, 262, 300);

  timeout = 0;
  while (MOTOR_RL->CommunicationIsWorking() == false)
  {
    //if( (timeout+=1) > 10 ) break;
    delay(50);
  }

  // 6
  tone(11, 262, 300);

  timeout = 0;
  while (MOTOR_RR->CommunicationIsWorking() == false)
  {
    //if( (timeout+=1) > 10 ) break;
    delay(50);
  }

  // 7
  tone(11, 262, 300);

  // Initial Configuration of the device and the Motor
  MOTOR_FL->SetCommandMode(SOLOMotorControllers::CommandMode::DIGITAL);
  MOTOR_FL->SetControlMode(SOLOMotorControllers::ControlMode::TORQUE_MODE);
  MOTOR_FR->SetCommandMode(SOLOMotorControllers::CommandMode::DIGITAL);
  MOTOR_FR->SetControlMode(SOLOMotorControllers::ControlMode::TORQUE_MODE);
  MOTOR_RL->SetCommandMode(SOLOMotorControllers::CommandMode::DIGITAL);
  MOTOR_RL->SetControlMode(SOLOMotorControllers::ControlMode::TORQUE_MODE);
  MOTOR_RR->SetCommandMode(SOLOMotorControllers::CommandMode::DIGITAL);
  MOTOR_RR->SetControlMode(SOLOMotorControllers::ControlMode::TORQUE_MODE);

  MOTOR_FL->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);
  MOTOR_FR->SetMotorDirection(SOLOMotorControllers::Direction::COUNTERCLOCKWISE);
  MOTOR_RL->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);
  MOTOR_RR->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);

  // 8
  tone(11, 300, 2000);
}

void loop()
{
  // get and publish IMU data
  sensors_event_t orientationData;
  IMU.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
  double imu_x = -1000000; //dumb values, easy to spot problem
  imu_x = orientationData.orientation.x;

  // set servo commands
  front_servo.write(servo_angle_front);
  rear_servo.write(servo_angle_rear);

  // set motor direction
  if(torque_request_fl < 0){
    MOTOR_FL->SetMotorDirection(SOLOMotorControllers::Direction::COUNTERCLOCKWISE);
  }else{
    MOTOR_FL->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);
  }

  if(torque_request_fr < 0){
    MOTOR_FR->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);
  }else{
    MOTOR_FR->SetMotorDirection(SOLOMotorControllers::Direction::COUNTERCLOCKWISE);
  }

  if(torque_request_rl < 0){
    MOTOR_RL->SetMotorDirection(SOLOMotorControllers::Direction::COUNTERCLOCKWISE);
  }else{
    MOTOR_RL->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);
  }

  if(torque_request_rr < 0){
    MOTOR_RR->SetMotorDirection(SOLOMotorControllers::Direction::CLOCKWISE);
  }else{
    MOTOR_RR->SetMotorDirection(SOLOMotorControllers::Direction::COUNTERCLOCKWISE);
  }

  // set motor torque
  /*wheel 1 to front 3 4 back
SOLO Obj1 => Wheel 1 ->CW FORWARD
SOLO Obj2 => Wheel 3 ->CW FORWARD
SOLO Obj4 => Wheel 2 ->CCW FORWARD
SOLO Obj3 => Wheel 4 ->CCW FORWARD
*/
  MOTOR_FL->SetTorqueReferenceIq(abs(torque_request_fl));
  MOTOR_FR->SetTorqueReferenceIq(abs(torque_request_fr));
  MOTOR_RL->SetTorqueReferenceIq(abs(torque_request_rl));
  MOTOR_RR->SetTorqueReferenceIq(abs(torque_request_rr));

  // relay actual motor torque
  torque_fb_fl = MOTOR_FL->GetQuadratureCurrentIqFeedback();
  torque_fb_fr = MOTOR_FR->GetQuadratureCurrentIqFeedback();
  torque_fb_rl = MOTOR_RL->GetQuadratureCurrentIqFeedback();
  torque_fb_rr = MOTOR_RR->GetQuadratureCurrentIqFeedback();

  rpm_fb_fl = MOTOR_FL->GetSpeedFeedback() * -1;
  rpm_fb_fr = MOTOR_FR->GetSpeedFeedback();
  rpm_fb_rl = MOTOR_RL->GetSpeedFeedback() * -1;
  rpm_fb_rr = MOTOR_RR->GetSpeedFeedback();

  battery_voltage = 0.25 * (MOTOR_FL->GetBusVoltage() + MOTOR_FR->GetBusVoltage() + MOTOR_RL->GetBusVoltage() + MOTOR_RR->GetBusVoltage());

  // PUBLISH CAR DATA
  car_data_array[0] = imu_x;
  car_data_array[1] = torque_fb_fl;
  car_data_array[2] = torque_fb_fr;
  car_data_array[3] = torque_fb_rl;
  car_data_array[4] = torque_fb_rr;
  car_data_array[5] = rpm_fb_fl;
  car_data_array[6] = rpm_fb_fr;
  car_data_array[7] = rpm_fb_rl;
  car_data_array[8] = rpm_fb_rr;
  car_data_array[9] = battery_voltage;
  out_msg.data = car_data_array;
  out_msg.data_length = 10;
  car_data_pub.publish( &out_msg );
  nh.spinOnce();
}

   
Quote
SOLO_Francesco
(@solo_francesco)
Chief Member
Joined: 2 years ago
Posts: 35
 

Hi,

are you using our Arduino library? did you update to the latest version 5.1.0?

 


   
ReplyQuote
Forum 1
(@lgroth)
Active Member
Joined: 1 month ago
Posts: 3
Topic starter  

@solo_francesco Hi,
thank you for the reply, yes we are using version 5.1.0 of the Arduino library. We have since tried our code in the Arduino IDE and PlatformIO, we have isolated a single motor controller and connected it directly to the Teensy, changed the baud rate and device ID. The issues persist with all configurations: The SOLO connects, works for a little bit, then disconnects, re-connects and so on.


   
ReplyQuote
Forum 1
(@lgroth)
Active Member
Joined: 1 month ago
Posts: 3
Topic starter  

We also hooked up an Oscilloscope to the TX and RX pins on the SOLO Pico side, there we observe that our messages arrive at the Pico, but when facing the above describes issues, we notice that there is no response from the Pico. Could there maybe be a buffer that overflows? The issues are present with all 4 Motorcontrollers so we assume it is not a hardware fault.


   
ReplyQuote
SOLO_Francesco
(@solo_francesco)
Chief Member
Joined: 2 years ago
Posts: 35
 

We can try to replicate the problem on our side using an Arduino in order to proper investigate it.

 

Can you make and share a simple of your code that generate the problem your are facing? 

Is better if the code is the little possible avoiding to consider non related part of it. For example it will happen when the code use only one SOLO, without other 3rd party libraries?  


   
ReplyQuote
Share: