Hello everyone,
So I am working on a robotics project that has two levels of control. The main control algorithms are written in MATLAB and lower-level control is developed to control driving and steering motors using Arduino.
The Matlab sends steering and speed commands to Arduino and using that Arduino will run the robot. The code for the driving motor is working fine but I am having trouble with the steering motor code. The main objective is that after the goal angle is sent in degrees the code will read the potentiometer value to get the current angle. then it should move according to the difference and then it should stop once the goal is reached. I was using a while loop to check if it has reached the desired angle but it turns out it is not a good idea as the code does not process anything until and unless that part is done. So how should I implement that part?
Now sometimes I also get an error in Matlab saying a timeout occurred during the write operation. I researched about it and it is because the read operation started before the write could finish. I have changed the baud rate and increased the buffer size for it but still the same thing. So I am very confused and need help with this.
In case you need any other information please let me know.
Thanks for your time and efforts!
The code is as follows:
// This is the Arduino test code Use code tags to format code for the forumfor 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 2:
//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;
int motor_speed = 0;
const double velocity_constant = 2 * 3.1415 * 0.0762;
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 = 1.2, 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 (!Serial) {} // Flash L4 if we are waiting for a serial connection
pinMode (SAB_ESTOP, OUTPUT);
if (Serial.available() > 0)
{
delta_req = Serial.parseFloat();
velocity_req = Serial.parseFloat();
if (Serial.read() == '$') //done transmitting
{
angle_req = delta_req;
desiredVelocity = velocity_req;
}
}
DriveMotor.motor(2, 127); // Brakes
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, SteerMotorValue); // Steering Motor
}
void loop() {
Serial.println("Code Started");
if (Serial.available() > 0) {
delta_req = Serial.parseFloat();
velocity_req = Serial.parseFloat();
Serial.flush();
if (Serial.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) {
brakes_on();
start_motors(0);
}
else {
brakes_off();
start_motors(desiredVelocity);
}
}
}
}
}
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);
}
void brakes_off() {
Serial.println("Brakes Disenaged");
DriveMotor.motor(2, 127);
}
void brakes_on() {
Serial.println("Brakes Engaged");
DriveMotor.motor(2, 0);
}
// Steer motor PID
void processMotor2(double angle)
{
potRest = (double)analogRead(SteerPot);
angle_act = angleCali(potRest);
// double angle_req_not_abs = angle;
double angle_req = angle;
double gap = angle_req - angle_act;
Serial.println(gap);
if (abs(gap) < 2 || abs(gap) > 40) {
SteerMotorValue = 0;
steering = false;
}
else {
myPIDSteer.SetTunings(consKp, consKi, consKd);
myPIDSteer.Compute();
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(1, 0);
Serial.println("Angle Reached");
steering = false;
}
else if (SteerMotorValue < 20) {
SteerMotorValue = 20;
}
if (abs(angleDifference) <= tolerance) {
// digitalWrite(SAB_ESTOP, LOW);
DriveMotor.motor(1, 0);
Serial.println("Angle Reached");
steering = false;
}
else if (angleDifference < 0) {
Serial.println("Going Right");
digitalWrite(SAB_ESTOP, HIGH);
DriveMotor.motor(1, -SteerMotorValue);
Serial.println(SteerMotorValue);
}
else {
Serial.println("Going Left");
digitalWrite(SAB_ESTOP, HIGH);
DriveMotor.motor(1, 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) 2023 - (324~490)
{
// double y = 0.26446281 * x - 154.446281;
double y = 0.39948 * x + -82.426; //2023
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);
}