ESP32 Forum

Code: Select all

#include "esp32-hal-ledc.h"
#include "esp32-hal-timer.h"

hw_timer_t * timer = NULL; //Created Hardware Timer

//Right Motor
#define rpwm 19 //pin A
#define rdir 18 //pin B

//Left Motor
#define lpwm 23 //pin A
#define ldir 22 //pin B

//Photo Interrupters
#define rightenc 4
#define leftenc 2

//Variables
unsigned long rightWheel;
unsigned long leftWheel;
double lenc = 0;
double renc = 0;
double finalleft = 0;

//Motor Specifications
const int freq = 50000;
//PWM Channel range : (0-7)
const int pwmChannel_left = 0;
const int pwmChannel_right = 1;
const int resolution = 16;

double speedo=62100; //Inital speed given to both motors (PWM value)
double error = 0; //Error between both motors
double errorSum=0; //Sum of all errors for intergrative part of PID
double finalError=0; //final error calculated
double kp = 0.5; //Proportionality constant (tune using trial and error)
double ki = 0.005; //Integrative constant
volatile byte PIDreval = LOW; //Flag to check if PID was corrected

//Timer Interrupt
void IRAM_ATTR TimerInt()
{
  PIDreval=HIGH;
  renc=rightWheel;
  lenc=leftWheel;
  error=renc-lenc;
 /* Serial.print("1.Error in Timer: ");
  Serial.println(error);*/
  //Sum of all errors for I part of PI
  errorSum+=error;
 /* Serial.print("2.Error Sum: ");
  Serial.println(errorSum);*/
  //Final PI formula
  finalError = (error*kp + errorSum*ki);
/*  Serial.print("3.Final: ");
  Serial.println(finalError);*/
  //Final speed of left wheel after correction
//  finalleft=(speedo+finalError);
 /* Serial.print("4.Final Left Speed: ");
  Serial.println(finalleft);*/
  //Reset count of both wheels to zero
  rightWheel=0;
  leftWheel=0;  
  /*if(finalleft>65136)
  finalleft=0;*/
}

void setup()
{
  Serial.begin(115200);

  //Timer
  timer = timerBegin(0,80,true);//(Use timer 0,80Mhz crystal thus set divider of 80 -> (1/(80MHz/80)))
  timerAttachInterrupt(timer,&TimerInt,true);//Attach timer function to timer
  timerAlarmWrite(timer,200000,true);//Set alarm to call timer fucntion every 1 second since 1 tick is 1 microsecond, third parameter is for it to repeat it
  timerAlarmEnable(timer);//enable timer
  Serial.println("Start Timer");
  
  pinMode(leftenc,INPUT_PULLUP);
  pinMode(lpwm,OUTPUT);
  pinMode(ldir,OUTPUT);

  pinMode(rightenc,INPUT_PULLUP);
  pinMode(rpwm,OUTPUT);
  pinMode(rdir,OUTPUT);

  attachInterrupt(4,rightEncoderISR,CHANGE);//Right Encoder Interrupt
  attachInterrupt(2,leftEncoderISR,CHANGE);//Left Encoder Interrupt

  //ledc setup
  ledcSetup(pwmChannel_left,freq,resolution);
  ledcAttachPin(lpwm,pwmChannel_left);

  ledcSetup(pwmChannel_right,freq,resolution);
  ledcAttachPin(rpwm,pwmChannel_right);
 
  Serial.println("*************************DC Motor Testing*************************");

  //Start Motor
  digitalWrite(rdir,LOW);
  digitalWrite(ldir,LOW);
  ledcWrite(pwmChannel_left,speedo);
  ledcWrite(pwmChannel_right,speedo);
  delay(1000);

}
void loop()
{  
  
  if(PIDreval==HIGH)
    {
      PIDreval = LOW; //Set it to LOW to update the errors
      finalleft=(speedo+finalError);

      //LOGS
/*      Serial.print("5.Right Speed: ");
      Serial.println(speedo);
      Serial.print("6.Left Speed :");
      Serial.println(finalleft);
      Serial.print("7.Error: ");
      Serial.println(error);
      Serial.print("8.Final Error: ");
      Serial.println(finalError);
      Serial.print("9.Left: ");
      Serial.println(lenc);
      Serial.print("10.Right: ");
      Serial.println(renc);
  */    
      ledcWrite(pwmChannel_left,finalleft);
    }
}
void rightEncoderISR()
{
  rightWheel++;
}
void leftEncoderISR()
{
  leftWheel++;
}