So sorry for late reply, I haven't looked at this forum for long time. Hardware was same, just wired it up normally with my diagram (shown earlier in this topic). Code to read encoder magnetic ticks (just change pins if needed [pinMode(blah]):
const int MOT1_ENC1 = 21;
const int MOT1_DIRECT = 20;
const int MOT2_ENC1 = 19;
const int MOT2_DIRECT = 18;
volatile int MOT1_ENC1_TICKS = 0;
volatile int MOT2_ENC1_TICKS = 0;
void ISR_A() {
if (digitalRead(MOT1_ENC1) == digitalRead(MOT1_DIRECT)) {
MOT1_ENC1_TICKS++;
} else {
MOT1_ENC1_TICKS--;
}
}
void ISR_B() {
if (digitalRead(MOT2_ENC1) == digitalRead(MOT2_DIRECT)) {
MOT2_ENC1_TICKS++;
} else {
MOT2_ENC1_TICKS--;
}
}
void setup() {
// pinMode(IN1, OUTPUT);
// pinMode(IN2, OUTPUT);
// pinMode(IN3, OUTPUT);
// pinMode(IN4, OUTPUT);
// pinMode(ENA, OUTPUT);
// pinMode(ENB, OUTPUT);
Serial.begin(9600);
attachInterrupt(digitalPinToInterrupt(MOT1_ENC1), ISR_A, CHANGE);
attachInterrupt(digitalPinToInterrupt(MOT2_ENC1), ISR_B, CHANGE);
}
void loop() {
Serial.println(MOT1_ENC1_TICKS);
Serial.println(MOT2_ENC1_TICKS);
Serial.println(" ");
}
Code I used with PID speed corrections, and code to run and ultrasonic sensor alongside your motors. Customize as wanted:
//#include <NewPing.h>
//#define TRIGGER_PIN 49
//#define ECHO_PIN 48
//#define MAX_DISTANCE 500
const int ENA = 13;
const int ENB = 12;
const int IN1 = 53;
const int IN2 = 52;
const int IN3 = 51;
const int IN4 = 50;
const int MOT1_ENC1 = 21;
const int MOT1_DIRECT = 20;
const int MOT2_ENC1 = 19;
const int MOT2_DIRECT = 18;
volatile long MOT1_TICKS = 0;
volatile long MOT2_TICKS = 0;
//volatile int MotorState = 0;
//volatile int UltrasonicState = 0;
//int ticks = 0;
//int Speed = 0;
//int SpeedPID = 0;
//long PIDtimer = 0;
//unsigned long waitForUltrasonic = 50;
//unsigned long timeOld = 0;
//unsigned long currentMillis = 0;
//float DURATION;
//float DISTANCE;
//NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
void ISR_A() {
if (digitalRead(MOT1_ENC1) == digitalRead(MOT1_DIRECT)) {
MOT1_TICKS++;
} else {
MOT1_TICKS--;
}
}
void ISR_B() {
if (digitalRead(MOT2_ENC1) == digitalRead(MOT2_DIRECT)) {
MOT2_TICKS++;
} else {
MOT2_TICKS--;
}
}
void Move_Forward(int Speed) {
MOT1_TICKS = 0;
MOT2_TICKS = 0;
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, Speed);
analogWrite(ENB, Speed);
MotorState = 1;
}
void Turn_Right(int Speed) {
MOT1_TICKS = 0;
MOT2_TICKS = 0;
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENA, Speed);
analogWrite(ENB, Speed);
MotorState = 1;
}
void Turn_Left(int Speed) {
MOT1_TICKS = 0;
MOT2_TICKS = 0;
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, Speed);
analogWrite(ENB, Speed);
MotorState = 1;
}
void Stop() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
void MotorStateMachine() {
switch (MotorState) {
case 0:
break;
case 1:
if (MOT1_TICKS >= ticks || MOT2_TICKS >= ticks) {
Stop();
MotorState = 0;
break;
}
if (PIDtimer < millis()) {
PIDtimer += 70;
SpeedPID = Speed + PID(MOT2_TICKS - MOT1_TICKS);
constrain(SpeedPID, 0, 255);
analogWrite(ENB, SpeedPID);
}
break;
case 2:
Stop();
MotorState = 0;
break;
case 3:
Stop();
delay(500);
ticks = CMtoTicks(30);
Turn_Right(180);
MotorState = 4;
break;
case 4:
if (MOT1_TICKS >= ticks || MOT2_TICKS >= ticks) {
Stop();
MotorState = 5;
break;
}
if (PIDtimer < millis()) {
PIDtimer += 70;
SpeedPID = Speed + PID(MOT2_TICKS - MOT1_TICKS);
constrain(SpeedPID, 0, 255);
analogWrite(ENB, SpeedPID);
}
break;
case 5:
delay(500);
ticks = CMtoTicks(1000);
UltrasonicState = 1;
MotorState = 6;
Move_Forward(180);
break;
case 6:
if (MOT1_TICKS >= ticks || MOT2_TICKS >= ticks) {
Stop();
MotorState = 7;
break;
}
if (PIDtimer < millis()) {
PIDtimer += 70;
SpeedPID = Speed + PID(MOT2_TICKS - MOT1_TICKS);
constrain(SpeedPID, 0, 255);
analogWrite(ENB, SpeedPID);
}
break;
case 7:
Stop();
ticks = 0;
MotorState = 0;
UltrasonicState = 0;
Stop();
break;
default:
break;
}
}
//void UltrasonicStateMachine() {
//switch (UltrasonicState) {
//case 0:
//break;
//case 1:
//currentMillis = millis();
//if ((currentMillis - timeOld) >= waitForUltrasonic) {
//timeOld = millis();
//DURATION = sonar.ping();
//DISTANCE = (DURATION / 2) * 0.0344;
//if (DISTANCE <= 30 && DISTANCE > 0) {
//MotorState = 3;
//UltrasonicState = 0;
//}
//}
//break;
//default:
//break;
//}
//}
int PID(int error) {
static int I = 0;
static int error_last = 0;
static float Kp = 3.9;
static float Ki = .001;
static float Kd = .1;
float PID;
I += error;
PID = Kp * error + Ki * I + Kd * (error - error_last);
error_last = error;
return int(PID);
}
int CMtoTicks(int cm) {
static int circumference = 65;
static int ticksPerCircum = 695;
float ticksPerCM = ticksPerCircum / circumference;
float CMtoTicks_dec = ticksPerCM * cm;
int CMtoTicks = CMtoTicks_dec;
return CMtoTicks;
}
void setup() {
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
attachInterrupt(digitalPinToInterrupt(MOT1_ENC1), ISR_A, CHANGE);
attachInterrupt(digitalPinToInterrupt(MOT2_ENC1), ISR_B, CHANGE);
//Put code to execute once here:
UltrasonicState = 1;
ticks = CMtoTicks(1000);
Move_Forward(180);
}
void loop() {
MotorStateMachine();
UltrasonicStateMachine();
}