Hello,
Newbie here. I have a super basic PD controller set up for a rotary motor. I can run it by sending a target position in the serial monitor using Serial.read(), which I can send while the program is running. My issue is, I'd like to see the graph of position and be able to send commands at the same time (to see the response visually). When I try to send commands through the Serial Plotter, nothing happens. The full code is here:
// Clockwise rotation direction.
#define CW 1
// Counter clockwise rotation direction.
#define CCW 0
// Frequency of output PWM signal.
#define PWM_FREQ 25000
// Update rate in microseconds.
#define CYCLE_TIME 1000
// Rate of sending position data to PC.
#define PLOT_RATE 200
#define PLOT_COUNTER CYCLE_TIME/PLOT_RATE
// IO pins. //
// The pin connected to ENBble A on the driver.
const int ENB = 14;
// Pins connected to IN3 and IN4 on the driver (for controlling the rotation direction).
const int IN4 = 15;
const int IN3 = 16;
// Signal A wire of the encoder.
const int ENCA = 17;
// Signal B wire of the encoder.
const int ENCB = 18;
// Value of ENCA.
int enca = 0;
// Value of ENCB.
int encb = 0;
// Value of IN3.
int in3 = 0;
// Value of IN4.
int in4 = 0;
// Motors position measure by encoder.
volatile long int motorPos = 0;
// Communication variables. //
// The byte sent over serial to Teensy.
int incomingByte = 0;
// Input buffer for receiving user input over serial.
char inputBuffer[8];
int bufferCnt = 0;
// Counter for sending position over serial for plotting purposes.
int pltCounter = 0;
// Controller variables./ /
// Last motor position.
long int lastPos = 0;
// Target motor position.
int targetPos = 0;
// Position at the start of the control loop.
int currentPos = 0;
// Position at the start of the previous control loop.
int prevPos = 0;
// Change in position (for approximating the derivative).
int dP = 0;
// Position error.
int pError = 0;
// P term of the controller.
int pTerm = 0;
// D term of the controller.
int dTerm = 0;
// Speed (= voltage = duty cycle). Controller output mapped to duty cycle range.
int spd = 0;
// Controller output.
int contOut = 0;
// Ratio for transforming counts to degrees (1920 count / 360 deg)
float ratio = static_cast<float>(360)/static_cast<float>(1920);
// Controller tunable parameters. //
// P gain.
const int kP = 10;
// D gain.
const int kD = 0;
// Error in encoder pulses correponding to the minimum duty cycle.
const int minErr = 0;
// Error in encoder pulses corresponding to the maximum duty cycle.
const int maxErr = 1024;
// minDutyCycle and maxDutyCycle depend on PWM frequency and can be determined in dc_motor_speed_control . For example for frequency of 25k,
// minDutyCycle = 120 (Motor starts to move),
// maxDutyCycle = 190 (Motor speed reaches its maximum given the supplied voltage).
const int minDutyCycle = 120;
const int maxDutyCycle = 190;
// Controller update rate variables. //
// Difference in time between desired cycle period and its execution time (without any delay()s).
int cycleDiff;
// Control loop start time.
long int startTime;
// Control loop end time.
long int endTime;
// Plotting
float motorPosDeg = 0;
//Plotter p;
void setup() {
Serial.begin(9600);
// Initialize the pins.
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(ENCA,INPUT);
pinMode(ENCB,INPUT);
analogWriteFrequency(ENB, PWM_FREQ);
// Set the initial rotation direction.
setDirection(CCW);
// Start with the motor at rest.
analogWrite(ENB,0);
// Encoder interrupt.
attachInterrupt(digitalPinToInterrupt(ENCA), encoderAISRising, RISING);
attachInterrupt(digitalPinToInterrupt(ENCB), encoderBISRising, RISING);
//p.Begin();
//p.AddTimeGraph("Position v Time", 1000, "Position", motorPosDeg);
}
// *** Encoder interrupt routines. See "Understanding Quadrature Encoded Signals" here: https://www.pjrc.com/teensy/td_libs_Encoder.html" *** //
void encoderAISRising(){
if(digitalRead(ENCB) == HIGH)
motorPos++;
else
motorPos--;
attachInterrupt(digitalPinToInterrupt(ENCA), encoderAISFalling, FALLING);
}
void encoderAISFalling(){
if(digitalRead(ENCB) == LOW)
motorPos++;
else
motorPos--;
attachInterrupt(digitalPinToInterrupt(ENCA), encoderAISRising, RISING);
}
void encoderBISRising(){
if(digitalRead(ENCA) == LOW)
motorPos++;
else
motorPos--;
attachInterrupt(digitalPinToInterrupt(ENCB), encoderBISFalling, FALLING);
}
void encoderBISFalling(){
if(digitalRead(ENCA) == HIGH)
motorPos++;
else
motorPos--;
attachInterrupt(digitalPinToInterrupt(ENCB), encoderBISRising, RISING);
}
// *** ***//
// Default rotation direction is CCW.
void setDirection(bool dir){
// CCW
if (dir == CCW){
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}else{
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
}
void loop() {
if (Serial.available() > 0) {
// Read the incoming bytes, until a next line character (Enter) is encountered.
while (1){
incomingByte = Serial.read();
// We have read all the bytes.
if (incomingByte == '\n' || incomingByte == '\r'){
Serial.read();
break;
}else{
// Store the byte in the buffer and move on to the next.
inputBuffer[bufferCnt] = incomingByte;
bufferCnt++;
}
}
// Add a NULL character to the end of the array. Required for using atoi.
inputBuffer[bufferCnt] = '\0';
bufferCnt = 0;
// Convert string to integer.
targetPos = atoi(inputBuffer);
targetPos = targetPos / ratio;
}
// int i = 0;
// if (i % 2 == 0){
// targetPos = 360;
// } else {
// targetPos = 0;
// }
startTime = micros();
// Get the latest motor position.
currentPos = motorPos;
// Position error.
//pError = targetPos - motorPos;
pError = targetPos - currentPos;
// P term of the controller.
pTerm = kP * pError;
dP = currentPos - prevPos;
// D term of the controller. CYCLE_TIME/1000 normalizes the denominator, otherwise dTerm would always be zero (integer division).
dTerm = kD * (dP/(CYCLE_TIME/1000));
contOut = pTerm + dTerm;
// Set the target duty cycle (i.e. speed (i.e. voltage)).
// Error (in terms of encoder pulses) in the range minErr-maxErr is mapped to speed range corresponding to minDutyCycle-maxDutyCycle.
// 4 parameters to tune here.
spd = map(abs(contOut),minErr,maxErr,minDutyCycle,maxDutyCycle);
// Set the direction according to sign of position error (CCW is positive), and then speed.
// One optimization would be calling analogWrite(ENB,abs(spd)) at the start or end of the loop instead
// (at the expense of readibility).
if (pError > 0){
setDirection(CCW);
analogWrite(ENB,abs(spd));
}else if (pError < 0){
setDirection(CW);
analogWrite(ENB,abs(spd));
}
if (pltCounter == PLOT_COUNTER){
float mtrPos = static_cast<float>(motorPos);
motorPosDeg = mtrPos * ratio;
Serial.print(int(motorPosDeg));
Serial.println();
pltCounter = 0;
}
pltCounter++;
prevPos = currentPos;
cycleDiff = micros() - startTime;
// Adjust the update rate.
if (cycleDiff < CYCLE_TIME){
delayMicroseconds(CYCLE_TIME - cycleDiff);
}
//i++;
}
It seems like a few other people have had this issue, but I cant find a fix. It seems like I could write additional code to plot it in some other program, but it seems like it should be doable in just the IDE. Any info about how to fix this (or at least why it doesn't work) would be super appreciated.