Purpose of the code:
The code is supossed to turn a DC gear motor by 1 revolutions for each loop, measure the X- and Y-coordinates on an imagined coordinate system via trigonemetry and then store these variables, so I can - later on - map them on mapping software.
Here is my code:
#include <PID_v1.h>
#include <Encoder.h>
#define trigPin 30
#define echoPin 31
//variables for distance measuring
float duration, Z;
//motor pins
double ccwRot1 = 6;
const double cwRot1 = 7;
//Encoder pins and functions
const int encoderPinA1 = 2;
const int encoderPinB1 = 4;
Encoder encoder1(encoderPinA1, encoderPinB1);
//Revolutions to Degrees
int rotPerCycle = 2500; //Number of pulses per 360 degrees
//standalone variables
int i = 0;
//Variables for the PID control loop
double Setpoint1, Input1, Output1;
PID motPoscw1(&Input1, &Output1, &Setpoint1, 1.5, 1.1, 2.5, DIRECT);
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
//PID loop settings
motPoscw1.SetMode(AUTOMATIC);
motPoscw1.SetSampleTime(1);
//Pins for the HC-sensor
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
} void loop() {
//For storing X- and Y-coords
float xCoord = 0;
float yCoord = 0;
//for choosing which case in measuring step
char lastCase = '0';
//PID loop1, it reads the encoder value and turns the motor clockwise untill angle change has been reached
float revs1 = encoder1.read();
double targetRot1 = 500;
Input1 = revs1;
Setpoint1 = targetRot1;
motPoscw1.Compute();
analogWrite(cwRot1, Output1);
Serial.println(revs1);
Serial.println(targetRot1);
if (targetRot1 < revs1) {
}
//pulse for activating the trigger pin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
//PulseIn measures the duration of the sound from the sensor to the object and back
duration = pulseIn(echoPin, HIGH);
//Formula for calculating distance
Z = (duration / 2) * 0.0343;
//if the motor has rotated sufficiently
if (targetRot1 == revs1) {
//For turning degrees into radians for trigonemtric calculations later on
float currentAngle1 = targetRot1 * 0.1444;
float angleAr = radians(currentAngle1);
float angleBr = radians(currentAngle1 - 90);
float angleCr = radians(currentAngle1 - 180);
float angleDr = radians(currentAngle1 - 270);
int quadrantValue = currentAngle1 / 91;
Serial.println(currentAngle1);
//If the rotary encoder has turned a full 360 degrees, it will be back to the first quadrant
if (quadrantValue > 3) {
currentAngle1 = currentAngle1 - 360;
} switch (quadrantValue) {
//Decide which quadrant of the coordinate system, the triangle is in
case 0:
lastCase = 'A';
break;
case 1:
lastCase = 'B';
break;
case 2:
lastCase = 'C';
break;
case 3:
lastCase = 'D';
break;
}
//Calculate the X- and Y-coordinates based on which quadrant, the triangle is in
switch (lastCase) {
case 'A':
yCoord = sin(angleAr) * Z;
xCoord = cos(angleAr) * Z;
break;
case 'B':
xCoord = -(sin(angleBr) * Z);
yCoord = cos(angleBr) * Z;
break;
case 'C':
xCoord = -(sin(angleCr) * Z);
yCoord = -(cos(angleCr) * Z);
break;
case 'D':
xCoord = (sin(angleDr) * Z);
yCoord = -(cos(angleDr) * Z);
break;
}
targetRot1 = targetRot1 + 500;
Serial.println("These are the Y-coords");
Serial.println(yCoord);
delay(500);
}
}
Setup:
I have a DC gear motor with a built in encoder. Nothing is wrong with my hardware, it is supplied by 12V, which is reccommended, and the encoder pulses correctly to the Arduino.
Problem 1:
My PID-loop will only hit the setpoint correctly 50% - or so - of the time. This seems weird, as the motor or it's sorroundings do not change?
Sometimes, it will hit the setpoint, progress through the loop, and once it gets to the bottom loop and prints "The Ycoordinates are:" it will overshoot?
Sometimes it will also undershoot by 1, but then it will just stay at 499.
Problem 2:
The loop is supposed to progress by incrimenting the setpoint by 1 at the end of the loop targetRot1 = targetRot1 + 1
But, whenever the PID actually hits the setpoint, it gets stuck in the loop and displays this message: