Hello everyone,
So I am trying to send some commands through MATLAB to Arduino Mega. I have used a 9 pin serial bus to USB cable to connect the Hardware Serial 3 of Arduino Mega to the computer. Now when the MATLAB code is running, the arduino starts running for a while and then stops responding. I checked using Serial Monitor and it shows that it is executing the commands but after some time it stops receiving commands. If I close the serial monitor and reopen it again, then the arduino again starts receiving the commands, executes them for a while and again stops receiving commands. I think after some while if (Serial3.available > 0 ) starts returning false. But why this happens and why does it start working as soon as I open the serial monitor? I don't know whether this a hardware problem or a software problem. Can someone please help me with it?
In case you need any other information, please let me know.
Thanks!
Please find the code below:
// This is the Arduino test code is for Robot_2, the one
// with one Sabertooth motor driver and two Odrive motor controllers.
//It will check if the robot is able to run and steer properly
//For Robot 1:
//Right = 381
//Centre = 472
//Left = 575
//30 for left and -30 for right
//updated the steering function
//added PID for steering
//added PID for driving motors
// ****************************************************
// Libraries
// ****************************************************
#include <SabertoothSimplified.h> // SaberTooth motor driver
#include <Sabertooth.h>
#include <SoftwareSerial.h> // Serial Library
#include "Arduino.h" // Arduino
#include <avr/io.h> // General definations of registers
#include <util/delay.h> // Delay functions
#include <PID_v1.h> // PID Library
#include <ODriveArduino.h>
#include <HardwareSerial.h>
// ****************************************************
// Hardware Pin Definitions
// ****************************************************
// Arduino Mega or Due - Serial1 and Serial2
// Serial1: ODrive1 (motor 1)
// Serial2: ODrive2 (motor 2)
HardwareSerial& odrive_serial1 = Serial1;
HardwareSerial& odrive_serial2 = Serial2;
#define SAB_RX 25 // not used
#define SAB_PORT 43 // SaberTooth S1
#define SAB_ESTOP 45 // SaberTooth S2
#define SteerPot A0 // Potentiomenter measuring the steering of the front wheels
// ****************************************************
// Define variables and constants
// ****************************************************
#define LOOPTIME 500000 // looptime = 0.5 s
unsigned long lastMicro = 0;
unsigned long lastMicroPrint = 0;
int baudrate = 19200; // Must match what you configure on the ODrive (see ODrive documentation)
bool running = false;
bool steering = false;
const double velocity_constant = 2 * 3.1415 * 0.0762;
int motor_speed = 0;
float delta_req = 0; // requested steering command -30~30 degree
float velocity_req = 0; // requested velocity command 0~5 m/s
double potRest, angle_act, SteerMotorValue, angle_req;
double speed_req, output_speed, desiredVelocity, speed_act;
// Define the aggressive and conservative Tuning Parameters
double consKp = 0.5, consKi = 0, consKd = 0.15;
// PID parameters for drive motors
double driveConsKp = 1, driveConsKi = 0.0, driveConsKd = 0.1;
// ****************************************************
// Initialize software serial and hardware Serial for the motor controller
// ****************************************************
SoftwareSerial SWSerial_Drive(SAB_RX, SAB_PORT);
SabertoothSimplified DriveMotor(SWSerial_Drive);
ODriveArduino odrive1(odrive_serial1); //left motor
ODriveArduino odrive2(odrive_serial2); //right motor
PID myPIDSteer(&angle_act, &SteerMotorValue, &angle_req, consKp, consKi, consKd, DIRECT);
PID myPIDDrive(&speed_act, &output_speed, &desiredVelocity, driveConsKp, driveConsKi, driveConsKd, DIRECT);
void setup() {
odrive_serial1.begin(baudrate);
odrive_serial2.begin(baudrate);
SWSerial_Drive.begin(9600);
Serial.begin(38400); // Serial to PC - Use this with MATLAB directly without Connecting anything
Serial3.begin(9600); //connect the usb serial port to use this with MATLAB
delay(10);
// Serial.flush(); // Clear
while (!Serial3) {} // Flash L4 if we are waiting for a serial connection
pinMode (SAB_ESTOP, OUTPUT);
if (Serial3.available() > 0)
{
delta_req = Serial3.parseFloat();
velocity_req = Serial3.parseFloat();
if (Serial3.read() == '$') //done transmitting
{
angle_req = delta_req;
desiredVelocity = velocity_req;
}
}
Serial.println("Waiting for ODrive1...");
while (odrive1.getState() == AXIS_STATE_UNDEFINED) {
delay(100);
}
Serial.println("ODrive1 connected!");
Serial.println("Waiting for ODrive2...");
while (odrive2.getState() == AXIS_STATE_UNDEFINED) {
delay(100);
}
Serial.println("ODrive2 connected!");
// Initialize ODrive controllers to closed-loop control mode
initializeODrive(odrive1);
initializeODrive(odrive2);
myPIDDrive.SetMode(AUTOMATIC);
potRest = (double)analogRead(SteerPot);
angle_act = angleCali(potRest);
angle_req = 0;
myPIDSteer.SetMode(AUTOMATIC);
myPIDSteer.SetOutputLimits(-127, 127);
DriveMotor.motor(1, 0);
DriveMotor.motor(2, SteerMotorValue);
// DriveMotor.motor(1, SteerMotorValue); // Steering Motor
}
void loop() {
Serial.println("Code Started");
if (Serial3.available() > 0) {
delta_req = Serial3.parseFloat();
velocity_req = Serial3.parseFloat();
if (Serial3.read() == '$') //done transmitting
{
desiredVelocity = ((velocity_req / velocity_constant) * 3);
Serial.println(desiredVelocity);
start_motors(desiredVelocity);
angle_req = delta_req;
potRest = (double)analogRead(SteerPot);
angle_act = angleCali(potRest);
processMotor2(angle_req);
PrintServoPosition(angle_req);
if ((micros() - lastMicro) >= LOOPTIME)
{
lastMicro = micros();
if (desiredVelocity <= 0) {
start_motors(0);
}
else {
start_motors(desiredVelocity);
}
}
}
}
}
void brakes_off() {
Serial.println("Brakes Disenaged");
DriveMotor.motor(1, 127);
}
void brakes_on() {
Serial.println("Brakes Engaged");
DriveMotor.motor(1, 0);
}
void start_motors(double speed_req) {
ODriveFeedback feedback1 = odrive1.getFeedback();
ODriveFeedback feedback2 = odrive2.getFeedback();
desiredVelocity = speed_req;
double speed_act = (double)feedback1.vel;
myPIDDrive.Compute();
Serial.println("_________");
Serial.println(output_speed);
odrive1.setVelocity(-output_speed);
odrive2.setVelocity(output_speed);
Serial.print("Motor 1 - pos:");
Serial.print(feedback1.pos);
Serial.print(", vel:");
Serial.print(feedback1.vel);
// delay(1000);
Serial.print(" - Motor 2 - pos:");
Serial.print(feedback2.pos);
Serial.print(", vel:");
Serial.print(feedback2.vel);
Serial.println();
}
void all_stop() {
odrive1.setVelocity(0);
odrive2.setVelocity(0);
}
// Steer motor PID
void processMotor2(double angle)
{
potRest = (double)analogRead(SteerPot);
angle_act = angleCali(potRest);
double angle_req = angle;
double gap = angle_req - angle_act;
Serial.println(gap);
if (abs(gap) < 2 || abs(gap) > 40) {
SteerMotorValue = 0;
// DriveMotor.motor(2, SteerMotorValue);
steering = false;
}
else {
myPIDSteer.SetTunings(consKp, consKi, consKd);
myPIDSteer.Compute();
// DriveMotor.motor(2, SteerMotorValue);
Serial.println(SteerMotorValue);
steering = true;
}
while (steering) {
double tolerance = 2.0;
double potValue = (double)analogRead(SteerPot);
angle_act = angleCali(potValue);
double angle_req = angle;
int angleDifference = angle_req - angle_act;
Serial.println("------------------");
Serial.println(angle_act);
Serial.println(angle_req);
Serial.println(angleDifference);
Serial.println("------------------");
if (SteerMotorValue == 0) {
DriveMotor.motor(2, 0);
Serial.println("Angle Reached");
steering = false;
}
else if (SteerMotorValue < 15 || SteerMotorValue > 40) {
SteerMotorValue = 20;
}
if (abs(angleDifference) <= tolerance) {
DriveMotor.motor(2, 0);
Serial.println("Angle Reached");
steering = false;
}
else if (angleDifference < 0) {
Serial.println("Going Right");
DriveMotor.motor(2, SteerMotorValue);
Serial.println(SteerMotorValue);
}
else {
Serial.println("Going Left");
DriveMotor.motor(2, -SteerMotorValue);
Serial.println(SteerMotorValue);
}
}
}
// Calibration of the SteerPot reading to front wheel steering angles
double angleCali(double x) // x is the reading of the TurnPot A0 (463~705)
{
// double y = 0.26446281 * x - 154.446281;
//double y = 0.32653 * x - 154.1224; //2023
double y = 0.32979 * x - 151.7042;
return (double)y; // return the current angle y, degrees
}
void PrintServoPosition(double orderAngle)
{
double potRest = (double)analogRead(SteerPot);
double currAngle = angleCali(potRest);
double angleError = orderAngle - currAngle;
Serial.print("Requested Angle: ");
Serial.print(orderAngle);
Serial.print(" Real Position:");
Serial.print(currAngle);
Serial.print(" Error:");
Serial.println(angleError);
//delay(400);
}
void initializeODrive(ODriveArduino & odrive) {
odrive.clearErrors();
// odrive.setState(AXIS_STATE_ENCODER_OFFSET_CALIBRATION);
// while (odrive.getState() != AXIS_STATE_IDLE) {
// delay(10);
// }
while (odrive.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(10);
}
start_motors(0);
}