#include <Encoder.h>
// Motor A
const byte ENA = 13;
const byte IN1 = 12;
const byte IN2 = 14;
// Motor B
const byte ENB = 25;
const byte IN3 = 27;
const byte IN4 = 26;
const byte A_0 = 36;
const byte A_1 = 39;
int xx = 0, yy = 0; // x&y potentiometers
Encoder left(19,16); // 32,33
Encoder right(18,17); // 34,35
void setup ()
{
// Declaramos todos los pines como salidas
pinMode (ENA, OUTPUT);
pinMode (ENB, OUTPUT);
pinMode (IN1, OUTPUT);
pinMode (IN2, OUTPUT);
pinMode (IN3, OUTPUT);
pinMode (IN4, OUTPUT);
pinMode (A_0, INPUT);
pinMode (A_1, INPUT);
Serial.begin(115200);
Serial.print("Encoder Positions\nleft, right, ave, diff, d_avg, d_diff\n");
}
void writeA(int a1, int a2) {
digitalWrite(IN1, a1);
digitalWrite(IN2, a2);
}
void writeB(int a3, int a4) {
digitalWrite(IN3, a3);
digitalWrite(IN4, a4);
}
void loop ()
{
static int lastXX, lastYY;
xx = (analogRead(A_0)-512) / 8;
yy = (analogRead(A_1) - 512) / 2;
int speedA = constrain(yy - xx, -255, 255);
int speedB = constrain(yy + xx, -255, 255);
if (speedA >= 0 ) {
writeA(1, 0);
analogWrite(ENA, speedA);
}
else
{
writeA(0, 1);
analogWrite(ENA, -speedA);
}
if (speedB >= 0 ) {
writeB(1, 0);
analogWrite(ENB, speedB);
}
else
{
writeB(0, 1);
analogWrite(ENB, -speedB);
}
if (false && (xx != lastXX || yy != lastYY)) {
lastXX = xx;
lastYY = yy;
Serial.print(xx);
Serial.print(",");
Serial.print(yy);
Serial.print(" ");
}
left.read();
right.read();
report();
}
void report(){
auto now = millis();
const typeof(now) interval = 1000;
static typeof(now) last = -interval;
static long last_avg, last_diff;
if(now - last >= interval){
last+= interval;
long lval = -left.read();
long rval= right.read();
long avg = (lval+rval)/2;
long diff = (lval-rval)/2;
long delta_avg = avg - last_avg;
long delta_diff = diff - last_diff;
last_avg = avg;
last_diff = diff;
Serial.print(lval);
Serial.print(" ");
Serial.print(rval);
Serial.print(" ");
Serial.print(avg);
Serial.print(" ");
Serial.print(diff);
Serial.print(" ");
Serial.print(delta_avg);
Serial.print(" ");
Serial.print(delta_diff);
Serial.println();
}
}
0x27 LCD