Hi,
I have written a code for the Arduino Mega which uses 2 serial ports, the default port is connected via USB A-B to my computer. This port is used to send the angle to my 3 motors, which use PID to control my motor position. What I am trying to do with the second serial port, ie. Serial3 is to read the encoder value and plot its graph onto the serial plotter. The problem is that every time I send a command to the motors, the serial plotter hangs momentarily. Also, when trying to plot all 3 motors at the same time, the graphs get mushed together into a big purple spectrum/band, and the changes which make it impossible to understand. Looking forward to your suggestions as to how I can properly plot the 3 graphs.
Note : I am sending the data to the motors via a processing UI
#include <PID_v1.h>
#include<Servo.h>
#define MotEnable 7 //Motor 110 rpm motor
#define MotFwd 50 // Motor Forward pin
#define MotRev 51
#define MotEnable1 4 // 60 rpm
#define MotFwd1 47 // Motor Forward pin
#define MotRev1 46
#define MotEnable2 8 //Motor Enable pin Runs on PWM signal
#define MotFwd2 48 // Motor Forward pin
#define MotRev2 49// Motor Reverse pin
int encoderPin1 = 18; //Encoder Output 'A' must connected with intreputpin of arduino.
int encoderPin2 = 19;
int encoderPin1_1 = 2; //Encoder Output 'A' must connected with intreputpin of arduino.
int encoderPin2_1 = 3;
int encoderPin1_2 = 20; //Encoder Output 'A' must connected with intreputpin of arduino.
int encoderPin2_2 = 21; //Encoder Otput 'B' must connected with intreput pin of arduino.
volatile int lastEncoded = 0; // Here updated value of encoder store.
volatile long encoderValue = 0;
volatile int lastEncoded1 = 0; // Here updated value of encoder store.
volatile long encoderValue1 = 0;
volatile int lastEncoded2 = 0; // Here updated value of encoder store.
volatile long encoderValue2 = 0;// Raw encoder value
const int PPR = 3960; //110 rpm
const int PPR1 = 7425; //60 rpm
const int PPR2 = 22275;// Encoder Pulse per revolution.
double kp = 10, ki = 0.00, kd = 0.1; // 110
double kp1 = 20, ki1 = 0.00, kd1 = 0.2;//60
double kp2 = 10, ki2 = 0.00, kd2 = 0.1;
double input = 0, output = 0, setpoint = 0;
double input1 = 0, output1 = 0, setpoint1 = 0;
double input2 = 0, output2 = 0, setpoint2 = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); //110
PID myPID1(&input1, &output1, &setpoint1, kp1, ki1, kd1, DIRECT); //60
PID myPID2(&input2, &output2, &setpoint2, kp2, ki2, kd2, DIRECT);
int angle = 360;
int angle1 = 360;
int angle2 = 360;// Maximum degree of motion.
int REV = 0;
int REV1 = 0;
int REV2 = 0; // Set point REQUIRED ENCODER VALUE
int lastMSB = 0;
int lastLSB = 0;
int lastMSB1 = 0;
int lastLSB1 = 0;
int lastMSB2 = 0;
int lastLSB2 = 0;
Servo gripperServo; // create servo object to control a servo
double x = 10.0;
double y = 10.0;
double L1 = 165.99; // L1 = 228mm
double L2 = 118.4; // L2 = 136.5mm
double theta1, theta2, phi, z;
const float zDistanceToSteps = 100;
byte inputValue[5];
int k = 0;
String content = "";
int data[10];
int theta1Array[100];
int theta2Array[100];
int phiArray[100];
int zArray[100];
int gripperArray[100];
int positionsCounter = 0;
void setup() {
gripperServo.attach(A0);
// initial servo value - open gripper
data[6] = 180;
gripperServo.write(data[6]);
delay(1000);
data[6] = 100;
gripperServo.write(data[6]);
delay(1000);
data[5] = 100;
pinMode(MotEnable, OUTPUT);
pinMode(MotFwd, OUTPUT);
pinMode(MotRev, OUTPUT);
pinMode(MotEnable1, OUTPUT);
pinMode(MotFwd1, OUTPUT);
pinMode(MotRev1, OUTPUT);
pinMode(MotEnable2, OUTPUT);
pinMode(MotFwd2, OUTPUT);
pinMode(MotRev2, OUTPUT);
Serial.begin(115200);
Serial3.begin(115200);//initialize serial comunication
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2, HIGH);
pinMode(encoderPin1_1, INPUT_PULLUP);
pinMode(encoderPin2_1, INPUT_PULLUP);
digitalWrite(encoderPin1_1, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2_1, HIGH);
pinMode(encoderPin1_2, INPUT_PULLUP);
pinMode(encoderPin2_2, INPUT_PULLUP);
digitalWrite(encoderPin1_2, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2_2, HIGH);//turn pullup resistor on
//call updateEncoder() when any high/low changed seen
//on interrupt 0 (pin 2), or interrupt 1 (pin 3)
attachInterrupt(digitalPinToInterrupt(18), updateEncoder, CHANGE);
attachInterrupt(digitalPinToInterrupt(19), updateEncoder, CHANGE);
attachInterrupt(digitalPinToInterrupt(2), updateEncoder_1, CHANGE);
attachInterrupt(digitalPinToInterrupt(3), updateEncoder_1, CHANGE);
attachInterrupt(digitalPinToInterrupt(20), updateEncoder_2, CHANGE);
attachInterrupt(digitalPinToInterrupt(21), updateEncoder_2, CHANGE);
//TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise
myPID.SetMode(AUTOMATIC); //set PID in Auto mode
myPID.SetSampleTime(1); // refresh rate of PID controller
myPID.SetOutputLimits(-225, 225);
myPID1.SetMode(AUTOMATIC); //set PID in Auto mode
myPID1.SetSampleTime(1); // refresh rate of PID controller
myPID1.SetOutputLimits(-225, 225);
myPID2.SetMode(AUTOMATIC); //set PID in Auto mode
myPID2.SetSampleTime(1); // refresh rate of PID controller
myPID2.SetOutputLimits(-225, 225);// this is the MAX PWM value to move motor, here change in value reflect change in speed of motor.
}
void loop() {
if (Serial.available()) {
content = Serial.readString(); // Read the incoming data from Processing
// Extract the data from the string and put into separate integer variables (data[] array)
for (int i = 0; i < 10; i++) {
int index = content.indexOf(","); // locate the first ","
data[i] = atoi(content.substring(0, index).c_str()); //Extract the number from start to the ","
content = content.substring(index + 1); //Remove the number from the string
}
}
/*data[0] - SAVE button status
data[1] - RUN button status
data[2] - Joint 1 angle
data[3] - Joint 2 angle
data[4] - Joint 3 angle
data[5] - Z position
data[6] - Gripper value
*/
// If RUN button is pressed
runMotor(data[2]);
runMotor1(data[3]);
runMotor2(data[5]);
//delay(100);
gripperServo.write(data[6]);
//delay(300);
}
void runMotor(int User_Input){
REV = map (User_Input, 0, angle, 0, PPR); // mapping degree into pulse
//Serial.print("this is REV - ");
Serial3.println(REV); // printing REV value
setpoint = REV; //PID while work to achive this value consider as SET value
input = encoderValue ; // data from encoder consider as a Process value
//Serial.print("encoderValue - ");
Serial3.println(encoderValue);
myPID.Compute();
//Serial.println(output);// calculate new output
pwmOut(output);
}
void runMotor1(int User_Input1){
REV1 = map (User_Input1, 0, angle1, 0, PPR1); // mapping degree into pulse
//Serial3.print("this is REV1 - ");
Serial3.println(REV1); // printing REV value
setpoint1 = REV1; //PID while work to achive this value consider as SET value
input1 = encoderValue1 ; // data from encoder consider as a Process value
//Serial3.print("encoderValue1 - ");
Serial3.println(encoderValue1);
myPID1.Compute();
//Serial.println(output);// calculate new output
pwmOut1(output1);
}
void runMotor2(int User_Input2){
REV2 = map (User_Input2, 0, angle2, 0, PPR2); // mapping degree into pulse
//Serial3.print("this is REV2 - ");
Serial3.println(REV2); // printing REV value
setpoint2 = REV2; //PID while work to achive this value consider as SET value
input2 = encoderValue2 ; // data from encoder consider as a Process value
//Serial3.print("encoderValue2 - ");
Serial3.println(encoderValue2);
myPID2.Compute();
//Serial.println(output);// calculate new output
pwmOut2(output2);
}
void pwmOut(int out) {
if (out > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable, out); // Enabling motor enable pin to reach the desire angle
forward(); // calling motor to move forward
}
else {
analogWrite(MotEnable, abs(out)); // if REV < encoderValue motor move in forward direction.
reverse(); // calling motor to move reverse
}
}
void pwmOut1(int out1) {
if (out1 > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable1, out1); // Enabling motor enable pin to reach the desire angle
forward1(); // calling motor to move forward
}
else {
analogWrite(MotEnable1, abs(out1)); // if REV < encoderValue motor move in forward direction.
reverse1(); // calling motor to move reverse
}
}
void pwmOut2(int out2) {
if (out2 > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable2, out2); // Enabling motor enable pin to reach the desire angle
forward2(); // calling motor to move forward
}
else {
analogWrite(MotEnable2, abs(out2)); // if REV < encoderValue motor move in forward direction.
reverse2(); // calling motor to move reverse
}
}
void updateEncoder(){
int MSB = digitalRead(encoderPin1); //MSB = most significant bit
int LSB = digitalRead(encoderPin2); //LSB = least significant bit
int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
int sum = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue ++;
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue --;
lastEncoded = encoded; //store this value for next time
}
void updateEncoder_1(){
int MSB1 = digitalRead(encoderPin1_1); //MSB = most significant bit
int LSB1 = digitalRead(encoderPin2_1); //LSB = least significant bit
int encoded1 = (MSB1 << 1) |LSB1; //converting the 2 pin value to single number
int sum1 = (lastEncoded1 << 2) | encoded1; //adding it to the previous encoded value
if(sum1 == 0b1101 || sum1 == 0b0100 || sum1 == 0b0010 || sum1 == 0b1011) encoderValue1 ++;
if(sum1 == 0b1110 || sum1 == 0b0111 || sum1 == 0b0001 || sum1 == 0b1000) encoderValue1 --;
lastEncoded1 = encoded1; //store this value for next time
}
void updateEncoder_2(){
int MSB2 = digitalRead(encoderPin1_2); //MSB = most significant bit
int LSB2 = digitalRead(encoderPin2_2); //LSB = least significant bit
int encoded2 = (MSB2 << 1) |LSB2; //converting the 2 pin value to single number
int sum2 = (lastEncoded2 << 2) | encoded2; //adding it to the previous encoded value
if(sum2 == 0b1101 || sum2 == 0b0100 || sum2 == 0b0010 || sum2 == 0b1011) encoderValue2 ++;
if(sum2 == 0b1110 || sum2 == 0b0111 || sum2 == 0b0001 || sum2 == 0b1000) encoderValue2 --;
lastEncoded2 = encoded2; //store this value for next time
}
void forward () {
digitalWrite(MotFwd, HIGH);
digitalWrite(MotRev, LOW);
}
void reverse () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, HIGH);
}
void forward1 () {
digitalWrite(MotFwd1, HIGH);
digitalWrite(MotRev1, LOW);
}
void reverse1 () {
digitalWrite(MotFwd1, LOW);
digitalWrite(MotRev1, HIGH);
}
void forward2 () {
digitalWrite(MotFwd2, HIGH);
digitalWrite(MotRev2, LOW);
}
void reverse2 () {
digitalWrite(MotFwd2, LOW);
digitalWrite(MotRev2, HIGH);
}