I'm trying to make the dc motor attached to encoder to stop at the setpoint but it doesn't work, any help please here's the code:
#define in1 4
#define in2 5
#define enA 3 // Analog output pin IPWM pin] connected to the L298N motor driver enable pin [enA]
// defines variables
volatile unsigned int counter = 0;
int angle;
float now_pid_error;
double kp = 1.1;
double ki = 0;
double kd = 1.2;
unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output, setPoint;
double cumError, rateError;
void setup() {
setPoint = 90;
Serial.begin (9600);
pinMode(4,OUTPUT);
pinMode(7,OUTPUT);
pinMode(6,OUTPUT);
pinMode(2, INPUT); // set pin to input
pinMode(3, INPUT); // set pin to input
digitalWrite(2, HIGH); // turn on pullup resistors
digitalWrite(3, HIGH); // turn on pullup resistors
//Setting up interrupt
//A rising pulse from encodenren activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on moust Arduino.
attachInterrupt(0, ai0, RISING);
//B rising pulse from encodenren activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on moust Arduino.
attachInterrupt(1, ai1, RISING);
}
void loop() {
Serial.println (counter);
if(counter>1199) {
counter = 0;
}
if(counter<0) {
counter = 1200;
}
angle = counter/3.3;
Serial.println (angle);
Serial.println (setPoint);
output = computePID(input);
Serial.println(output);
// wait 2 milliseconds before the next loop for the analog-to-digital
// converter to settle after the last reading:
delay(30);
if (output >0) { //Verify that the variable contains information
digitalWrite(5,LOW);
digitalWrite(4,HIGH); // here input data is store in integer form
}
if (output <0) { //Verify that the variable contains information
digitalWrite(4,LOW);
digitalWrite(5,HIGH); // here input data is store in integer form
}
if (output ==0) { //Verify that the variable contains information
digitalWrite(4,LOW);
digitalWrite(5,LOW); // here input data is store in integer form
}
}
double computePID(double inp){
currentTime = millis(); //get current time
elapsedTime = (double)(currentTime - previousTime); //compute time elapsed from previous computation
error = setPoint - angle; // determine error
cumError += error * elapsedTime; // compute integral
rateError = (error - lastError)/elapsedTime; // compute derivative
double out = kp*error + ki*cumError + kd*rateError; //PID output
lastError = error; //remember current error
previousTime = currentTime; //remember current time
return out; //have function return the PID output
}
void ai0() {
// ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
// Check pin 3 to determine the direction
if(digitalRead(3)==LOW) {
counter++;
}else{
counter--;
}
}
void ai1() {
// ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
// Check with pin 2 to determine the direction
if(digitalRead(2)==LOW) {
counter--;
}else{
counter++;
}
}
You forgot to say what is wrong with what your sketch does.
I think it will be hard to hit your target when the only options are full-speed-forward and full-speed-reverse. Typically you would use the output of your PID to control both the speed and direction of the motor.
You seem to be using Pin 3 for both an encoder input and a PWM output for the motor. You should pick a different PWM pin for the motor speed.
You forgot to set 'input' to the current position ('angle'?) before passing it to the PID. I assume it stays at 0.0 and your PID tries to move 'forward' to get to the setpoint (90.0).
I think you will find the time it takes for the ultrasonic to measure and output a distance, is not instant, it takes time.
I would think the distance to a moving object would not be 100% accurate.
What is the target that you are sensing distance to?
Can you please tell us what your project is?
Why an ultrasonic sensor and not a limit switch?
don't see an ultrasonic sensor being used.
where is input measured?
if (output >0) { //Verify that the variable contains information
digitalWrite(5,LOW);
digitalWrite(4,HIGH); // here input data is store in integer form
}
if (output <0) { //Verify that the variable contains information
digitalWrite(4,LOW);
digitalWrite(5,HIGH); // here input data is store in integer form
}
if (output ==0) { //Verify that the variable contains information
digitalWrite(4,LOW);
digitalWrite(5,LOW); // here input data is store in integer form
}
doesn't this defeat the usage of the PID output which should drive the motor using PWM?
isn't the above simply setting the motor direction or disabling the motor?
is "braking" needed?
double kp = 1.1;
double ki = 0;
double kd = 1.2;
double out = kp*error + ki*cumError + kd*rateError;
think about what the above results in assuming the output is the measure of some correction, most often the speed.
the rateError is a measure of speed toward some target. with kd > 0, the output will be proportional to the speed approaching the object. wouldn't it make sense that kd should be negative so that the output compensates for the speed of approach?
i don't believe there's any need for kp in a digital system. i believe it's typically needed to maintain a voltage on a motor to maintain its speed when it is at some desired target speed. so just work with kp and -kd
this is the new code i just want it to stop at 90 degree
#define in1 7
#define in2 8
#define enA 6 // Analog output pin IPWM pin] connected to the L298N motor driver enable pin [enA]
// defines variables
volatile unsigned int counter = 0;
int angle;
float now_pid_error;
double kp = 2;
double ki = 1;
double kd = 0.01;
unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output, setPoint;
double cumError, rateError;
void setup() {
setPoint = 90;
Serial.begin (9600);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
pinMode(2, INPUT); // set pin to input
pinMode(3, INPUT); // set pin to input
digitalWrite(2, HIGH); // turn on pullup resistors
digitalWrite(3, HIGH); // turn on pullup resistors
//Setting up interrupt
//A rising pulse from encodenren activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on moust Arduino.
attachInterrupt(0, ai0, RISING);
//B rising pulse from encodenren activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on moust Arduino.
attachInterrupt(1, ai1, RISING);
}
void loop() {
Serial.println (counter);
if(counter>1199) {
counter = 0;
}
if(counter<0) {
counter = 1200;
}
angle = counter/3.3;
Serial.println (angle);
Serial.println (setPoint);
output = computePID(input);
Serial.println(output);
// wait 2 milliseconds before the next loop for the analog-to-digital
// converter to settle after the last reading:
delay(30);
if (output >0) { //Verify that the variable contains information
digitalWrite(7,LOW);
digitalWrite(8,HIGH); // here input data is store in integer form
}
if (output <0) { //Verify that the variable contains information
digitalWrite(8,LOW);
digitalWrite(7,HIGH); // here input data is store in integer form
}
if (output ==0) { //Verify that the variable contains information
digitalWrite(7,LOW);
digitalWrite(8,LOW); // here input data is store in integer form
}
}
double computePID(double inp){
currentTime = millis(); //get current time
elapsedTime = (double)(currentTime - previousTime); //compute time elapsed from previous computation
error = setPoint - angle; // determine error
cumError += error * elapsedTime; // compute integral
rateError = (error - lastError)/elapsedTime; // compute derivative
double out = kp*error + ki*cumError + kd*rateError; //PID output
lastError = error; //remember current error
previousTime = currentTime; //remember current time
return out; //have function return the PID output
}
void ai0() {
// ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
// Check pin 3 to determine the direction
if(digitalRead(3)==LOW) {
counter++;
}else{
counter--;
}
}
void ai1() {
// ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
// Check with pin 2 to determine the direction
if(digitalRead(2)==LOW) {
counter--;
}else{
counter++;
}
}
I edited the code but it's doesn't stop at 90 degree what's wrong?
#define in1 7
#define in2 8
#define enA 6 // Analog output pin IPWM pin] connected to the L298N motor driver enable pin [enA]
// defines variables
volatile unsigned int counter = 0;
int angle;
float now_pid_error;
double kp = 2;
double ki = 1;
double kd = 0.01;
unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output, setPoint;
double cumError, rateError;
void setup() {
setPoint = 90;
Serial.begin (9600);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
pinMode(2, INPUT); // set pin to input
pinMode(3, INPUT); // set pin to input
digitalWrite(2, HIGH); // turn on pullup resistors
digitalWrite(3, HIGH); // turn on pullup resistors
//Setting up interrupt
//A rising pulse from encodenren activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on moust Arduino.
attachInterrupt(0, ai0, RISING);
//B rising pulse from encodenren activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on moust Arduino.
attachInterrupt(1, ai1, RISING);
}
void loop() {
Serial.println (counter);
if(counter>1199) {
counter = 0;
}
if(counter<0) {
counter = 1200;
}
angle = counter/3.3;
Serial.println (angle);
Serial.println (setPoint);
output = computePID(input);
Serial.println(output);
// wait 2 milliseconds before the next loop for the analog-to-digital
// converter to settle after the last reading:
delay(30);
if (output >0) { //Verify that the variable contains information
digitalWrite(7,LOW);
digitalWrite(8,HIGH); // here input data is store in integer form
}
if (output <0) { //Verify that the variable contains information
digitalWrite(8,LOW);
digitalWrite(7,HIGH); // here input data is store in integer form
}
if (output ==0) { //Verify that the variable contains information
digitalWrite(7,LOW);
digitalWrite(8,LOW); // here input data is store in integer form
}
}
double computePID(double inp){
currentTime = millis(); //get current time
elapsedTime = (double)(currentTime - previousTime); //compute time elapsed from previous computation
error = setPoint - angle; // determine error
cumError += error * elapsedTime; // compute integral
rateError = (error - lastError)/elapsedTime; // compute derivative
double out = kp*error + ki*cumError + kd*rateError; //PID output
lastError = error; //remember current error
previousTime = currentTime; //remember current time
return out; //have function return the PID output
}
void ai0() {
// ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
// Check pin 3 to determine the direction
if(digitalRead(3)==LOW) {
counter++;
}else{
counter--;
}
}
void ai1() {
// ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
// Check with pin 2 to determine the direction
if(digitalRead(2)==LOW) {
counter--;
}else{
counter++;
}
}
Hi,
Can you make a simple code to run the motor very slow and see if you can actually stop in the same place each time?
How precise must the setpoint be?
Lets check that an ultrasonic system can be precise.
The important bit is the ultrasonic, where was it mounted and what was it looking at?
If your friend is using a clear plastic disc, a photo interrupter would probably be better.
PID needs to know where the setpoint is, so it can approach it and slow to a stop.
In this project the encoder is only capable of showing direction, speed, and RELATIVE position, it doesn't know were the setpoint is until it detects it.
Hi,
90 with respect to where, where does the motor think it is when turned ON?
The encoder does not know when first turned on where the setpoint on the disk is.
So how does it know what direction or how far to turn the motor, or how fast, the only time it will know when its at the setpoint is when it appears, not as it approaches which is what PID needs.
That is why normally an external limit switch is used., in this case the OP is trying to use ultrasonics.
Depending on how it is setup depends on how it responds.
Setpoint 90, with PID, will probably result in the motor turning 90 counts and stopping from where it started, and not the setpoint which could be at 200 counts.
There no specs on the encoder, the motor or driver.
The picture gives some clues.
Sorry but if the OP wants to find setpoint, it needs to be done blindly, slowly until it is detected.
Then you can sync the encoder output and use PID to then locate positions.
I'm talking about the whole system.
You cannot put in the code that the setpoint is at 90.
How does the code know where it is, you haven't established any benchmark yet.
That is why you do a slow rotation until you find the setpoint on the disk.
Then you use that as the reference.
PID will not speed up the process of finding the setpoint on the disk.
It is unfortunate that this forum is not communicating directly with the project developer, @dido4max is doing a good job, but having to double handle makes the solution and explaining the problems with it makes it slow.
'counter' is unsigned. It will never be less than zero.
OK. So 'input' is never used.
You're still not using PWM to control motor speed. Try changing:
if (output > 0) //Verify that the variable contains information
{
digitalWrite(7, LOW);
digitalWrite(8, HIGH); // here input data is store in integer form
}
if (output < 0) //Verify that the variable contains information
{
digitalWrite(8, LOW);
digitalWrite(7, HIGH); // here input data is store in integer form
}
if (output == 0) //Verify that the variable contains information
{
digitalWrite(7, LOW);
digitalWrite(8, LOW); // here input data is store in integer form
}
to:
if (output > 0.9) // Forward
{
analogWrite(6, constrain(output, 0, 255));
digitalWrite(7, LOW);
digitalWrite(8, HIGH); // here input data is store in integer form
}
else if (output < -0.9) // Backward
{
analogWrite(6, constrain(-output, 0, 255));
digitalWrite(8, LOW);
digitalWrite(7, HIGH); // here input data is store in integer form
}
else // Stop
{
analogWrite(6, 0);
digitalWrite(7, LOW);
digitalWrite(8, LOW); // here input data is store in integer form
}
I can't see your motor from here. I can't tell what your motor is doing. You are saying that the sketch "work's now" so what does it do? Does the motor not stop? Does the motor stop but not 90° from where it started (position 0)?
the new project is simple (dc motor with encoder) I'm trying to make the motor stop at 90 degree from 0 . i said it "work's" when i edited the code with your code but now it doesn't stop at all, so i need to use pulses or position to make it stop at 90 degree ?
#define in1 7
#define in2 8
#define enA 6 // Analog output pin IPWM pin] connected to the L298N motor driver enable pin [enA]
// defines variables
volatile unsigned int counter = 0;
int angle;
float now_pid_error;
double kp = 2;
double ki = 1;
double kd = 0.01;
unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output, setPoint;
double cumError, rateError;
void setup() {
setPoint = 90;
Serial.begin (9600);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
pinMode(2, INPUT); // set pin to input
pinMode(3, INPUT); // set pin to input
digitalWrite(2, HIGH); // turn on pullup resistors
digitalWrite(3, HIGH); // turn on pullup resistors
//Setting up interrupt
//A rising pulse from encodenren activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on moust Arduino.
attachInterrupt(0, ai0, RISING);
//B rising pulse from encodenren activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on moust Arduino.
attachInterrupt(1, ai1, RISING);
}
void loop() {
Serial.println (counter);
if(counter>1199) {
counter = 0;
}
if(counter<0) {
counter = 1200;
}
angle = counter/3.3;
Serial.println (angle);
Serial.println (setPoint);
output = computePID(input);
Serial.println(output);
// wait 2 milliseconds before the next loop for the analog-to-digital
// converter to settle after the last reading:
delay(30);
if (output > 0.9) // Forward
{
analogWrite(6, constrain(output, 0, 255));
digitalWrite(7, LOW);
digitalWrite(8, HIGH); // here input data is store in integer form
}
else if (output < -0.9) // Backward
{
analogWrite(6, constrain(-output, 0, 255));
digitalWrite(8, LOW);
digitalWrite(7, HIGH); // here input data is store in integer form
}
else // Stop
{
analogWrite(6, 0);
digitalWrite(7, LOW);
digitalWrite(8, LOW); // here input data is store in integer form
}
}
double computePID(double inp){
currentTime = millis(); //get current time
elapsedTime = (double)(currentTime - previousTime); //compute time elapsed from previous computation
error = setPoint - angle; // determine error
cumError += error * elapsedTime; // compute integral
rateError = (error - lastError)/elapsedTime; // compute derivative
double out = kp*error + ki*cumError + kd*rateError; //PID output
lastError = error; //remember current error
previousTime = currentTime; //remember current time
return out; //have function return the PID output
}
void ai0() {
// ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
// Check pin 3 to determine the direction
if(digitalRead(3)==LOW) {
counter++;
}else{
counter--;
}
}
void ai1() {
// ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
// Check with pin 2 to determine the direction
if(digitalRead(2)==LOW) {
counter--;
}else{
counter++;
}
}
