This is the code I tested with that does work (although it has the issue of incrementing the DC motor imprecisely, but that's besides the point) however it prints everything constantly on the same line instead of new line even though I have NL & Return carriage enabled in serial.
I'm using RS485 with Arduino Leonardo on a custom board, just the bare minimums required on the Leonardo to make it work.
#include <PID_v1.h>
//Motor definitions and variables
#define MotorA_CW_pin 10
#define MotorA_CCW_pin 9
boolean motorAstopped=true;//true -> motor A is stopped. false -> Motor A is running
//Encoder definitions and variables
#define steps_per_mm 307 //how many steps should encoder count to move 1mm (was 307) //------ CAN BE CHANGED BY USER
#define debounce_del 2 //debounce time needed for the encoder //------ CAN BE CHANGED BY USER
#define EncoderA_pin 2 //encoder is connected to this pin in MCU
#define fix_param 0
int stepsenc;//stores how many pulses encoder needs to move distancemm
unsigned long encoder_stepsA=0;
unsigned long encoder_stepsB=0;
int lasthigh=2;//stores encoder status, 0 -> low, 1 -> high, 2-> invalid
int distancemm=2;//distance in mm to move //------ CAN BE CHANGED BY USER
boolean SAR;//Stores encoder value (low or high)
unsigned long tmr_debounce;//Stores the time remaining until debounce_del is finished
//Serial communication definitions and variables
#define serial_enable_pin A5
boolean debugEN=true;//Activate or deactivate serial data debug
//PID VALUES
double Setpoint, Input, Output;
double Kp=100, Ki=500, Kd=50; //------ CAN BE CHANGED BY USER
//If component does not reach enough distance, increase P, if it moves larger distance than needed, increase I
// Kp=50, Ki=100, Kd=0;
// P = POWER
// I and D = How much decreament in power should happen when motor is about to reach destination
// I mainly affects power
// D mainly afects smoothness
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
unsigned long tmrzz,tmryy;
int err[50];
boolean onebrake=true;
void setup() {
for(int i=0;i<50;i++){
err[i]=0;
}
Serial.begin(57600);
pinMode(MotorA_CW_pin,OUTPUT);
pinMode(MotorA_CCW_pin,OUTPUT);
pinMode(serial_enable_pin,OUTPUT);
analogWrite(MotorA_CW_pin,0);
analogWrite(MotorA_CCW_pin,0);
pinMode(EncoderA_pin, INPUT_PULLUP);
pinMode(A4,INPUT_PULLUP);
//attachInterrupt(digitalPinToInterrupt(EncoderA_pin), Read_encoderA, CHANGE);
myPID.SetMode(AUTOMATIC); //set PID in Auto mode
myPID.SetSampleTime(1); // refresh rate of PID controller
myPID.SetOutputLimits(0,255); ///////////////////////// PID POWER (0 - 255)
}
void loop() {
stepsenc=distancemm*steps_per_mm;
Setpoint = stepsenc - (encoder_stepsB - fix_param);
write_data_serial("Setpoint: ");
write_data_serial(String(Setpoint));
write_data_serial("\n");
encoder_stepsB=0;
motorAstopped=false;
lasthigh=2;
encoder_stepsA=0;
Input=0;
tmrzz=millis();
onebrake=true;
while(tmrzz+3000>millis()){
if(encoder_stepsA<Setpoint){
myPID.Compute();
if(encoder_stepsA>Setpoint*0.85){
//Output=Output*0.9;
}
analogWrite(MotorA_CW_pin,Output);
tmrzz=millis(); //loop timer
}
else if(onebrake==true){
analogWrite(MotorA_CW_pin,0);
analogWrite(MotorA_CCW_pin,255);
tmryy==millis();
onebrake=false;
}
SAR=digitalRead(2);
if(SAR==true && lasthigh!=1 && tmr_debounce+debounce_del<=micros()){
encoder_stepsA++;
//if(encoder_stepsA>=stepsenc){break;}
lasthigh=1;
tmr_debounce=micros();
}
else if(SAR==false && lasthigh!=0 && tmr_debounce+debounce_del<=micros()){
encoder_stepsA++;
//if(encoder_stepsA>=stepsenc){break;}
lasthigh=0;
tmr_debounce=micros();
}
Input=encoder_stepsA;
if(onebrake==false && tmryy+50<=millis()){ ////////////// BRAKE TIME is 12500 uS
analogWrite(MotorA_CCW_pin,0);
}
}
analogWrite(MotorA_CW_pin,0);
encoder_stepsB=encoder_stepsA-Setpoint;
write_data_serial("Actual Steps: ");
write_data_serial(String(encoder_stepsA));
write_data_serial("\n");
write_data_serial("Error: ");
write_data_serial(String(encoder_stepsB));
write_data_serial("\n");
err[0]=encoder_stepsB;
encoder_stepsA=0;
Input=0;
delay(2000);
}
void Read_encoderA(){
encoder_stepsA++;
if(encoder_stepsA>=stepsenc){
analogWrite(MotorA_CW_pin,255);
motorAstopped=true;
}
delay(debounce_del);//debounce time
}
//This function receives String of data, and writes it on serial for debugging
void write_data_serial(String dat){
if(debugEN==true){
digitalWrite(serial_enable_pin,HIGH);
delay(10);
Serial.print(dat);
delay(10);
digitalWrite(serial_enable_pin,LOW);
delay(100);
}
}