Hello,
I have a feedback system that consists of a 5 V battery pack, a UNO, a ULN2003 drive, a 28BJ-48 stepper and a KY040 rotary encoder. The shaft of the encoder is connected to the one of the motor, so that every revolution of the motor's shaft corresponds to one revolution of the encoder's.
For background I will try to explain first what I achieved so far:
My first bit of code (Program A) has the following operation and works fine:
- Initialize current encoder position (y) at 0
- Begin serial communication and wait for an integer number as input (r).
- Calculate error as e = r - y
- Move CKW while e>0, CCKW while e<0, stay idle while e == 0
- Using interrupts update y and e (either +1 or -1) every time the encoder moves one step
- Iterate from step 4)
My aim is to create a new code (Program B) which allows me to track a non-constant reference signal.
I did this by defining r as an array instead, which for simplicity I first defined as an array made up of only the value 20 (eventually I will have to track a signal of randomly generated values - I am open to suggestions if there is a better way of doing this). I then introduced a for loop to check at every iteration the current value of r, so that practically the previous operation would iterate from step 2). In theory, the motor should rotate 20 encoder steps, as if I inputted 20 in Program A, however what actually occurs is that the motor starts to vibrate but the shaft never fully turns. I believe the issue is the fact that by using interrupts the entire operation is halted when the encoder is between a step (stepper increment is lower than encoders), however I am not sure and cannot think of an alternative anyways. What do you think?
Program 2:
// ================= Declarations =================
// Encoder
const int PinCLK = 2; // Encoder CLK (White)
const int PinDT = 3; // Encoder DT (Brown)
int pos_last = 0; // Last position
volatile int y = 0; // Current position
// Motor
int Pin1 = 8; // Driver pin 1 (Yellow)
int Pin2 = 9; // Driver pin 2 (Orange)
int Pin3 = 10; // Driver pin 3 (Green)
int Pin4 = 11; // Driver pin 4 (Blue)
int v = 1; // Motor speed (1 max speed)
int r[24] = {20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20};
boolean ref_bool = false;
int i = 0;
int e = 1; // Tracking error
// ================= Functions =================
void idle (){
digitalWrite(Pin1, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin4, LOW);
}
void CCKW (){
// 1
digitalWrite(Pin1, HIGH);
digitalWrite(Pin2, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin4, LOW);
delay(v);
// 2
digitalWrite(Pin1, HIGH);
digitalWrite(Pin2, HIGH);
digitalWrite(Pin3, LOW);
digitalWrite(Pin4, LOW);
delay (v);
// 3
digitalWrite(Pin1, LOW);
digitalWrite(Pin2, HIGH);
digitalWrite(Pin3, LOW);
digitalWrite(Pin4, LOW);
delay(v);
// 4
digitalWrite(Pin1, LOW);
digitalWrite(Pin2, HIGH);
digitalWrite(Pin3, HIGH);
digitalWrite(Pin4, LOW);
delay(v);
// 5
digitalWrite(Pin1, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin3, HIGH);
digitalWrite(Pin4, LOW);
delay(v);
// 6
digitalWrite(Pin1, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin3, HIGH);
digitalWrite(Pin4, HIGH);
delay (v);
// 7
digitalWrite(Pin1, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin4, HIGH);
delay(v);
// 8
digitalWrite(Pin1, HIGH);
digitalWrite(Pin2, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin4, HIGH);
delay(v);
}
void CKW(){
// 1
digitalWrite(Pin4, HIGH);
digitalWrite(Pin3, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin1, LOW);
delay(v);
// 2
digitalWrite(Pin4, HIGH);
digitalWrite(Pin3, HIGH);
digitalWrite(Pin2, LOW);
digitalWrite(Pin1, LOW);
delay (v);
// 3
digitalWrite(Pin4, LOW);
digitalWrite(Pin3, HIGH);
digitalWrite(Pin2, LOW);
digitalWrite(Pin1, LOW);
delay(v);
// 4
digitalWrite(Pin4, LOW);
digitalWrite(Pin3, HIGH);
digitalWrite(Pin2, HIGH);
digitalWrite(Pin1, LOW);
delay(v);
// 5
digitalWrite(Pin4, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin2, HIGH);
digitalWrite(Pin1, LOW);
delay(v);
// 6
digitalWrite(Pin4, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin2, HIGH);
digitalWrite(Pin1, HIGH);
delay (v);
// 7
digitalWrite(Pin4, LOW);
digitalWrite(Pin3, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin1, HIGH);
delay(v);
// 8
digitalWrite(Pin4, HIGH);
digitalWrite(Pin3, LOW);
digitalWrite(Pin2, LOW);
digitalWrite(Pin1, HIGH);
delay(v);
}
// Interrupt
void isr () {
static unsigned long t_interrupt_last = 0;
unsigned long t_interrupt = millis();
// If interrupts come faster than 5ms, assume it's a bounce and ignore
if (t_interrupt - t_interrupt_last > 5) {
if (digitalRead(PinDT) == LOW)
{
y-- ;
}
else {
y++ ;
}
}
// Keep track of when we were here last (no more than every 5ms)
t_interrupt_last = t_interrupt;
}
// Track: If the current rotary switch position has changed then update everything
void track() {
if (y != pos_last) {
// Keep track of this new value
pos_last = y;
}
}
// ================= Setup =================
void setup() {
Serial.begin(9600);
// Encoder
pinMode(PinCLK,INPUT);
pinMode(PinDT, INPUT);
// Motor
pinMode(Pin1, OUTPUT);
pinMode(Pin2, OUTPUT);
pinMode(Pin3, OUTPUT);
pinMode(Pin4, OUTPUT);
// Attach the routine to service the interrupts
attachInterrupt(digitalPinToInterrupt(PinCLK), isr, LOW);
}
// ================= Loop =================
void loop() {
for(i=1;i<24;i=i+1) { // Iterate through array
if(e<0) {
track();
CKW();
e = r[i] - y;
i=i+1;
}
if(e>0) {
track();
CCKW();
e = r[i] - y;
i=i+1;
}
if(e==0) {
track();
idle();
e = r[i] - y;
i=i+1;
}
}
}
