Hello everyone,
I am building a 7DoF robot with servos and steppers. I wrote an Arduino sketch to move the motors and a GUI on Processing to control them with sliders.
I believe I am supposed to run both sketches together but im facing the following problem with the USB Port:
I first upload the Arduino sketch through Port COM4. Then when I try to run the Processing sketch It says error "Port busy".
Instead if I run the Processing sketch with Arduino IDE closed, it works but then After opening Arduino IDE (to use serial monitor to check Serial prints so i know if theres any communication), the Serial Monitor cant connect to Arduino for the same reason: "Port busy".
What exactly am I supposed to do?
This is the Arduino sketch:
#include <AccelStepper.h>
#include <Servo.h>
// Stepper NEMA17 controlled through CNC Shield V3 with DRV8825 drivers (1/16 microstepping)
AccelStepper stepper1(AccelStepper::DRIVER, 2, 5); // X module on shield
AccelStepper stepper2(AccelStepper::DRIVER, 3, 6); // Y module on shield
AccelStepper stepper3(AccelStepper::DRIVER, 4, 7); // Z module on shield
AccelStepper stepper4(AccelStepper::DRIVER, 12, 13); // A module on shield
Servo needleServo; // Servo360 to move Needle
Servo ServoX; // Servo360 to move x axis
Servo ServoY; // Servo360 to move y axis
int servospeedforward = 100;
int servospeedbackward = 80;
float servorevpersecond = 3;
void setup() {
Serial.begin(115200);
// Config Stepper speed and acceleration
stepper1.setMaxSpeed(200);
stepper1.setAcceleration(100);
stepper2.setMaxSpeed(200);
stepper2.setAcceleration(100);
stepper3.setMaxSpeed(200);
stepper3.setAcceleration(100);
stepper4.setMaxSpeed(200);
stepper4.setAcceleration(100);
needleServo.attach(9); // Attach Servo to pin 9 (Needle control)
ServoX.attach(10); // Attach Servo to pin 10 (X axis control)
ServoY.attach(11); // Attach Servo to pin 11 (Y axis control)
}
void loop() {
if (Serial.available() > 0) {
String data = Serial.readStringUntil('\n');
int j1 = data.substring(0, data.indexOf(',')).toInt();
data = data.substring(data.indexOf(',') + 1);
int j2 = data.substring(0, data.indexOf(',')).toInt();
data = data.substring(data.indexOf(',') + 1);
int j3 = data.substring(0, data.indexOf(',')).toInt();
data = data.substring(data.indexOf(',') + 1);
int z = data.substring(0, data.indexOf(',')).toInt();
data = data.substring(data.indexOf(',') + 1);
int needle = data.substring(0, data.indexOf(',') + 1).toInt();
data = data.substring(data.indexOf(',') + 1);
int servoXVal = data.substring(0, data.indexOf(',')).toInt();
data = data.substring(data.indexOf(',') + 1);
int servoYVal = data.toInt();
Serial.println(j1);
// Set Stepper target angle and move them
stepper1.moveTo(j1);
stepper2.moveTo(j2);
stepper3.moveTo(j3);
stepper4.moveTo(z);
stepper1.run();
stepper2.run();
stepper3.run();
stepper4.run();
// Servo [mm] to [ms] (of running) conversion
float needle_ms = ((abs(needle) / 4) / servorevpersecond) * 1000; // 4 = screw lead = 4mm per revolution
float servoXVal_ms = ((abs(servoXVal) / 4) / servorevpersecond) * 1000;
float servoYVal_ms = ((abs(servoYVal) / 4) / servorevpersecond) * 1000;
// Move servo motors
if (needle > 0) {
needleServo.write(servospeedforward); // run forward
Serial.println("servo needle running forward");
delay(needle_ms);
needleServo.write(90); // stop
}
if (needle < 0) {
needleServo.write(servospeedbackward); // run backward
Serial.println("servo needle running backwards");
delay(needle_ms);
needleServo.write(90); // stop
}
if (servoXVal > 0) {
ServoX.write(servospeedforward); // run forward
Serial.println("servo X running forward");
delay(servoXVal_ms);
ServoX.write(90); // stop
}
if (servoXVal < 0) {
ServoX.write(servospeedbackward); // run backward
Serial.println("servo X running backward");
delay(servoXVal_ms);
ServoX.write(90); // stop
}
if (servoYVal > 0) {
ServoY.write(servospeedforward); // run forward
Serial.println("servo Y running forward");
delay(servoYVal_ms);
ServoY.write(90); // stop
}
if (servoYVal < 0) {
ServoY.write(servospeedbackward); // run backward
Serial.println("servo Y running backward");
delay(servoYVal_ms);
ServoY.write(90); // stop
}
}
}
This Is the Processing sketch:
import processing.serial.*;
import controlP5.*;
Serial myPort;
ControlP5 cp5;
int j1Slider = 0;
int j2Slider = 0;
int j3Slider = 0;
int zSlider = 0;
int needleValue = 0;
int xSlider = 0;
int ySlider = 0;
int slider1Previous = 0;
int slider2Previous = 0;
int slider3Previous = 0;
int sliderzPrevious = 0;
int needleValuePrevious = 0;
int sliderxPrevious = 0;
int slideryPrevious = 0;
float q1_offset = 100; //[mm]
float q1, q4; //[mm]
float q0, q2, q3; //[deg]
float a1 = 52.5; //[mm]
float a2 = 80;
float a3 = 104;
float j1, j2, j3, x, y, z, n;
float rhoP;
float xP = 0;
float yP = 0;
float zP = 0;
String data;
void setup() {
size(960, 800);
myPort = new Serial(this, "COM4", 115200);
cp5 = new ControlP5(this);
PFont pfont = createFont("Arial", 25, true);
ControlFont font = new ControlFont(pfont, 22);
// Joint1 control
cp5.addSlider("j1Slider")
.setPosition(110, 190)
.setSize(270, 30)
.setRange(-75, 75) // [deg]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("J1");
// Joint2 control
cp5.addSlider("j2Slider")
.setPosition(110, 315)
.setSize(270, 30)
.setRange(0, 60) // [deg]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("J2");
// Joint3 control
cp5.addSlider("j3Slider")
.setPosition(110, 440)
.setSize(270, 30)
.setRange(-45, 45) // [deg]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("J3");
// Z axis control
cp5.addSlider("zSlider")
.setPosition(110, 565)
.setSize(270, 30)
.setRange(0, 170) // [mm]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("Z");
// Needle control
cp5.addSlider("needleValue")
.setPosition(110, 690)
.setSize(270, 30)
.setRange(0, 40) // [mm]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("Needle");
// X axis control
cp5.addSlider("xSlider")
.setPosition(500, 565)
.setSize(270, 30)
.setRange(-50, 50) // [mm]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("X");
// Y axis control
cp5.addSlider("ySlider")
.setPosition(500, 690)
.setSize(270, 30)
.setRange(-50, 50) // [mm]
.setColorLabel(#3269c2)
.setFont(font)
.setCaptionLabel("Y");
}
void draw() {
background(#F2F2F2);
textSize(26);
fill(33);
text("Venipuncture Robot Control", 260, 60);
textSize(22);
text("Joint 1", 35, 250);
text("Joint 2", 35, 375);
text("Joint 3", 35, 500);
text("Axis Z", 35, 625);
text("Needle", 35, 750);
text("Axis X", 35, 875);
text("Axis Y", 35, 1000);
fill(33);
textSize(32);
text("X: " + nf(xP, 0, 2), 500, 290);
text("Y: " + nf(yP, 0, 2), 650, 290);
text("Z: " + nf(zP, 0, 2), 800, 290);
updateData();
if (slider1Previous != j1Slider || slider2Previous != j2Slider || slider3Previous != j3Slider || sliderzPrevious != zSlider || needleValuePrevious != needleValue || sliderxPrevious != xSlider || slideryPrevious != ySlider) {
j1 = round(cp5.getController("j1Slider").getValue());
j2 = round(cp5.getController("j2Slider").getValue());
j3 = round(cp5.getController("j3Slider").getValue());
z = round(cp5.getController("zSlider").getValue());
n = round(cp5.getController("needleValue").getValue());
x = round(cp5.getController("xSlider").getValue());
y = round(cp5.getController("ySlider").getValue());
forwardKinematics();
myPort.write(data); // Send data to Arduino
}
slider1Previous = j1Slider;
slider2Previous = j2Slider;
slider3Previous = j3Slider;
sliderzPrevious = zSlider;
needleValuePrevious = needleValue;
sliderxPrevious = xSlider;
slideryPrevious = ySlider;
}
void forwardKinematics() {
q0 = j1;
if (q0 < 0) {
q0 = q0 + 360;
}
q1 = z;
q2 = j2 - 45;
if (q2 < 0) {
q2 = q2 + 360;
}
q3 = 180 - j3 + j2;
q4 = needleValue;
rhoP = a2*cos(q2) + (a3+q4)*cos(q3);
zP = q1_offset + q1 - a1 + a2*sin(q2) + (a3+q4)*sin(q3);
xP = rhoP*cos(q0) + x;
yP = rhoP*sin(q0) + y;
}
void updateData() {
data = String.format("%d,%d,%d,%d,%d,%d,%d\n", (j1Slider-slider1Previous), (j2Slider-slider2Previous), (j3Slider-slider3Previous), (zSlider-sliderzPrevious), (needleValue-needleValuePrevious), (xSlider-sliderxPrevious), (ySlider-slideryPrevious));
}