dcmotor_encoder.ino

#include <util/atomic.h> // For the ATOMIC_BLOCK macro

#define ENCA 2 // YELLOW
#define ENCB 3 // WHITE
#define PWM 10
#define IN2 7
#define IN1 8

volatile int posi = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
float eprev = 0;
float eintegral = 0;

void setup() {
  Serial.begin(9600);
  Serial.print( F("Compiled: "));
  Serial.print( F(__DATE__));
  Serial.print( F(" Time: "));
  Serial.println( F(__TIME__));
  Serial.print( F("File: "));
  Serial.println( F(__FILE__));
  Serial.println( F(" "));
  
  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
  
  pinMode(PWM,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  
  Serial.println("target   pos");
}

int TARGET_DURATION = 6000;
int target_list[] = {-2000,2000};
int target_counter = 0;
unsigned long previousMillis = 0;

void loop() {
  int target = target_list[target_counter]; 
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= TARGET_DURATION) {
    target_counter = (target_counter + 1) % (sizeof(target_list) / sizeof(target_list[0]));
    previousMillis = currentMillis;
    target = target_list[target_counter];    
  }
  // set target position
  //int target = 200;
  //int target = 400*sin(prevT/1e6);  // was 250 ******

  // PID constants
  float kp = 2.500;    //  was   1.0
  float kd = 0.000;    //  was   0.025
  float ki = 0.000;   //        0.0

  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;

  // Read the position in an atomic block to avoid a potential
  // misread if the interrupt coincides with this code running
  // see: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
  int pos = 0; 
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    pos = posi;
  }
  
  // error
  int e = pos - target;

  // derivative
  float dedt = (e-eprev)/(deltaT);

  // integral
  eintegral = eintegral + e*deltaT;

  // control signal
  float u = kp*e + kd*dedt + ki*eintegral;

  // motor power
  float pwr = fabs(u);
  if( pwr > 255 ){
    pwr = 150;   //   was 255   *******
  }

  // motor direction
  int dir = 1;
  if(u<0){
    dir = -1;
  }

  // signal the motor
  setMotor(dir,pwr,PWM,IN1,IN2);


  // store previous error
  eprev = e;

  Serial.print(target);
  Serial.print("         ");
  Serial.print(pos);
  Serial.println();
}

void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}

void readEncoder(){
  int b = digitalRead(ENCB);
  if(b > 0){
    posi++;
  }
  else{
    posi--;
  }
}