Using long, since my application uses a lot of encoder ticks
in a need of double, because the PID wants it
how to convert, or omit using long / having to give double ?
//full code
#include <digitalWriteFast.h>
#include <PID_v1.h>
// library for high performance reads and writes by jrraines
// 40000 encoder ticks per second per motor is tested and working
// Quadrature encoders
#define c_LeftEncoderInterrupt 0
#define c_LeftEncoderPinA 2
#define c_LeftEncoderPinB 3
#define LeftEncoderIsReversed
volatile bool _LeftEncoderBSet;
volatile long _LeftEncoderTicks;
//DC motor
const int INA = 12; const int INA2 = 6;
const int PWM_PIN = 11; const int PWM_PIN2 = 5;
const int INB = 10; const int INB2 = 4;
int speedm = 0;
//PID
double Setpoint = 10000, Input = int((_LeftEncoderTicks)), Output, Kp = 0.0025, Ki = 0.0008, Kd = 0.0008;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
void setup()
{
Serial.begin(115200);
// Quadrature encoders
pinMode(c_LeftEncoderPinB, INPUT); // sets pin B as input
digitalWrite(c_LeftEncoderPinB, LOW); // turn on pullup resistorsM
pinMode(c_LeftEncoderPinA, INPUT); // sets pin A as input
digitalWrite(c_LeftEncoderPinA, LOW); // turn on pullup resistors
attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
//DC motor
pinMode(INA, OUTPUT); pinMode(INA2, OUTPUT);
pinMode(INB, OUTPUT); pinMode(INB2, OUTPUT);
pinMode(PWM_PIN, OUTPUT); pinMode(PWM_PIN2, OUTPUT);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Serial.println(Input);
// Serial.println("\t");
delay(5);
if (_LeftEncoderTicks <= Setpoint) {
digitalWrite(INA, HIGH); digitalWrite(INA2, HIGH);
digitalWrite(INB, LOW); digitalWrite(INB2, LOW);
Serial.println("forward");
myPID.Compute();
analogWrite(PWM_PIN, Output); analogWrite(PWM_PIN2, Output);
}
/*
if (_LeftEncoderTicks >= Setpoint) {
digitalWrite(INB, HIGH); digitalWrite(INB2, HIGH);
digitalWrite(INA, LOW); digitalWrite(INA2, LOW);
Serial.printlnM("backward");
myPID.Compute();
analogWrite(PWM_PIN, Output); analogWrite(PWM_PIN2, Output);
}
*/
}
// Interrupt service routines for the motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
_LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
#else
_LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
#endif
}