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++;
}Surya