//#include <Arduino_FreeRTOS.h>
#include <PID_v2.h> //SYSTEM PARAMETER - PID_v1 Compatible Library
#include <LiquidCrystal_I2C.h> //SYSTEM PARAMETER - ESP32 LCD Compatible Library (By: Robojax)
#include <PubSubClient.h> //SYSTEM PARAMETER - MQTT Library (By: Adafruit)
/**************************************************************************
// Motor Model : JGA25/371
// Voltage Range : 6-24 V
// Voltage Rated : 12.0 V
// Speed : 500 RPM
// Max Load : 300 mA
// Current Max : 1 A
// Ratio : 9.28
// RED : + Motor
// Black : - Encoder Power (3.3-5V)
// Yellow : Encoder A
// Green : Encoder B
// Blur : + Encoder Power ( 3.3-5V)
// While : - Motor
velocity = 1.0, // m/s
Ma_kp = 1.25, // PID Motor A
Ma_ki = 0.001,
Ma_kd = 0.00,
Mb_kp = 1.25,
Mb_ki = 0.001,
Mb_kd = 0.00,
**************************************************************************/
/**************************************************************************
// Motor Model : CHR-GM25/370
// Voltage Range : 6-24 V
// Voltage Rated : 12.0 V
// Max Load : 300 mA
// Current Max : 1 A
// Reduction Ratio : 1 : 20
// Rated Torque (kg.cm) : 3.5
// Rated Torque (N.m) : 0.34
// Rated Speed : 350 RPM
// Hall resolution (PPR) : 224.5
// Pulse output of rotation : Basic pusle 11PPR x Gear reduction ratio
// Basic pusle : 11PPR
// Holzer power supply : DC 3.3 / DC 5.0 V
// Output signal type : Square wave AB phase
// Number of trigger poles
off magnetic ring : 22 poles (11 pairs of poles)
// Encoder type : AB dual phase incremental encoder
// RED : + Motor (Pin 1)
// Black : - Encoder Power (GND) (Pin 2)
// Yellow : Encoder B phase (Pin 3)
// Green : Encoder A phase (Pin 4)
// Blur : + Encoder Power ( 3.3-5V) (Pin 5)
// While : - Motor (Pin 6)
// Outer diameter : 25 mm / Lmm
// Axle diameter : Length 12mm, D long 8 mm
//
velocity = 1.0, // m/s
Ma_kp = 1.125, // PID Motor A
Ma_ki = 0.0125,
Ma_kd = 0.00010,
Mb_kp = 1.125,
Mb_ki = 0.0125,
Mb_kd = 0.00010, // PID Motor B
**************************************************************************/
#define CW (1)
#define CCW (-1)
#define STOP (0)
#define NMOTORS (2) // How many motors
#define M0 (0) // Motor 0
#define M1 (1) // Motor 1
#define M2 (2) // Motor 2
#define M3 (3) // Motor 3
//#define UNO (1)
#define ESP (1)
#ifdef UNO
#define ENC_A1 (16) // Encoder M0 A1
#define ENC_A2 (17) // Encoder M0 A2
#define PWM_A (18)
#define INT_A1 (4) // Direction ++ M0
#define INT_A2 (2) // Direction -- M0
#define ENC_B1 (14) // Encoder M0 B1
#define ENC_B2 (15) // Encoder M0 B2
#define PWM_B (19) // PWM M1
#define INT_B1 (25) // Direction ++ M1
#define INT_B2 (26) // Direction -- M1
#endif
#ifdef ESP
#define ENC_A1 (16) // Encoder M0 A1
#define ENC_A2 (17) // Encoder M0 A2
#define PWM_A (18)
#define INT_A1 (4) // Direction ++ M0
#define INT_A2 (2) // Direction -- M0
#define ENC_B1 (14) // Encoder M0 B1
#define ENC_B2 (15) // Encoder M0 B2
#define PWM_B (19) // PWM M1
#define INT_B1 (25) // Direction ++ M1
#define INT_B2 (26) // Direction -- M1
#endif
const int enca[] = {ENC_A1, ENC_B1}; // Set pin encoder A
const int encb[] = {ENC_A2, ENC_B2}; // Set pin encoder B
const int pwm [] = {PWM_A, PWM_B }; // Set pin PWM A, B
const int in1 [] = {INT_A1, INT_B1}; // Set pin direction A
const int in2 [] = {INT_A2, INT_B2}; // Set pin direction B
#define Umax (255) // Max speed motor
#define Debounce (3) //SW debounce
#define STOP (0)
#define START (1)
#define adc_min (0) // Analog contol minimum value
#define adc_max (100) // Analog contol maximum value
#define adc_res_min (0) // ADC resulution 2^11 bit minimum value
#define adc_res_max (4095) // ADC resulution 2^11 bit maximum value
#define gain_k_min (0) // Analog contol minimum value
#define gain_k_max (10) // Analog contol maximum value
#define InputPin (32)
#define InPin analogRead(InputPin)
#define Step_rev map(InPin,adc_res_min,adc_res_max,adc_min,adc_max)
#define KiPin (33)
#define SetKi analogRead(KiPin)
#define Step_ki map(SetKi,adc_res_min,adc_res_max,gain_k_min,gain_k_max)
#define KpPin (34)
#define SetKp analogRead(KpPin)
#define Step_kp map(SetKp,adc_res_min,adc_res_max,gain_k_min,gain_k_max)
#define KdPin (35)
#define SetKd analogRead(KdPin)
#define Step_kd map(SetKd,adc_res_min,adc_res_max,gain_k_min,gain_k_max)
#define SWITCH_ON (12) //Pin D18 SWITCH ON
#define SWITCH_DIR (13) //Pin D19 SWITCH OFF
#define BUTTON_START digitalRead(SWITCH_ON) == 1 // Set Input Pin
#define BUTTON_DIR digitalRead(SWITCH_DIR)== 1 // Set Input Pin
/**************************************************************************
**************************************************************************/
// Globals
long prevT = 0;
volatile int
target[] = {0,0,0,0},
target_f[] = {0,0,0,0},
posi[] = {0,0,0,0},
pos[] = {0,0,0,0},
time_H = 0,
time_L = 0,
Frequency = 0,
SampleRate = 10;
int encoder0Pos = 0,
ENC_A1Last = 0,
n = 0,
Motor_dir = 0,
Forward = 1,
Backward = -1;
double
Setpoint[] = {0,0,0,0},
Input[] = {0,0,0,0},
Output[] = {0,0,0,0},
/*********Hard PID*******************************************/
/*
Ma_kp = 1.75, // PID Motor A 1.25
Ma_ki = 0.0125,
Ma_kd = 0.00011,
Mb_kp = 1.75,
Mb_ki = 0.0125,
Mb_kd = 0.00011, // PID Motor B 1.25
*/
/*********Soft PID*******************************************/
Ma_kp = 1.0, // PID Motor A 1.25
Ma_ki = 0.000011,
Ma_kd = 0.00000011,
Mb_kp = 1.0,
Mb_ki = 0.000011,
Mb_kd = 0.00000011, // PID Motor B 1.25
Kp[] = {Ma_kp, Mb_kp}, // PID Motors
Ki[] = {Ma_ki, Mb_ki},
Kd[] = {Ma_kd, Mb_kd},
aggKp = 1.0,
aggKi = 0.000011,
aggKd = 0.00000011,
consKp = 0.5,
consKi = 0.000005,
consKd = 0.00000005;
float
t_period = 0.0,
velocity = 1.0, // m/s
Turn = 20.0, // 20 ratation / 1 output shaft rotation
pulses = 11.0, // 11 pulses / 1 rotation (PPR)
pulsesPerTurn = pulses*Turn, // 11*20 = 220 pulses / Turn
Meter = 3.5714, // circle / wheel size 28 cm (1/0.28)
pulsesPerMeter = pulsesPerTurn*Meter, // 220 * (1/0.28) = 785.70 pulses / Meter
rpm[] = {0,0},
outputpwm[] = {0,0},
positionChange[] = {0.0,0.0,0.0,0.0};
volatile unsigned int event = 1;
volatile unsigned long lastime = 0;
volatile char Serial_input_read; // Buffer Serial input
/*********Struct command Control *******/
volatile struct command{
union{
unsigned short WORD;
struct {
// MSB High Byte
unsigned char :8;
// MSB Low Byte
unsigned char ButtonDir :1;
unsigned char Button :1;
unsigned char Rotate :1;
unsigned char Enable :1;
unsigned char Dir :1;
unsigned char Reset :1;
unsigned char Emer :1;
unsigned char Run :1;
}BIT;
}CTRL;
}CMD;
/**************************************************************************
// PID class instances
// P_ON_M specifies that Proportional on Measurement be used
// P_ON_E (Proportional on Error) is the default behavior
// if motor will only run at full speed try ‘REVERSE’ instead of ‘DIRECT’
**************************************************************************/
PID M0PID(&Input[M0], &Output[M0], &Setpoint[M0], Kp[M0], Ki[M0], Kd[M0], DIRECT);
PID M1PID(&Input[M1], &Output[M1], &Setpoint[M1], Kp[M1], Ki[M1], Kd[M1], DIRECT);
LiquidCrystal_I2C lcd(0x3F,16,2); //SYSTEM PARAMETER - Configure LCD RowCol Size and I2C Address (0x27, 0x3F)
/**************************************************************************
* ---- limits for sampling period -----------
**************************************************************************/
const float
T0MAX = 1, // controller (sec)
T0MIN = 0.001;
const int
T0MSMAX = 1000, // Task0 (ms)
T0MSMIN = 1,
T1MSMAX = 1000, // Task1
T1MSMIN = 1,
T2MSMAX = 1000, // Task2
T2MSMIN = 1,
T3MSMAX = 1000, // Task3
T3MSMIN = 1,
T4MSMAX = 1000, // Task4
T4MSMIN = 1,
T5MSMAX = 10000, // Task5
T5MSMIN = 1,
T6MSMAX = 20000, // Task6
T6MSMIN = 1;
float
Tsampling = 0.01, // sampling period
tdata = 0; // time data sent to output
int
T0_ms = 0,
T1_ms = 0,
T2_ms = 0,
T3_ms = 0,
T4_ms = 0,
T5_ms = 0,
T6_ms = 0,
T7_ms = 0,
T0ticks = 0,
T1ticks = 0,
T2ticks = 0,
T3ticks = 0,
T4ticks = 0,
T5ticks = 0,
T6ticks = 0,
T7ticks = 0,
Tloop_ms = 500, // loop period
baudRate = 115200; // USER PARAMETER - USB Serial Baud Rate (bps)
/**************************************************************************
// -----------FreeRTOS handles and flags-----------------
**************************************************************************/
TaskHandle_t xTask0h = NULL,
xTask1h = NULL,
xTask2h = NULL,
xTask3h = NULL;
TaskHandle_t xTask5h = NULL,
xTask4h = NULL,
xTask6h = NULL,
xTask7h = NULL;
QueueHandle_t xQueue_y = NULL,
xQueue_u = NULL;
bool xQueueOK = LOW;
portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED;
/**************************************************************************
// create tasks and queues
***************************************************************************/
void freertos_init(void)
{
xQueueOK = LOW; // start wit false
xQueue_y = xQueueCreate(1000,sizeof(float));
xQueue_u = xQueueCreate(1000,sizeof(float));
if ((xQueue_y == NULL)|(xQueue_u == NULL)) Serial.println("Error creating the queue");
else xQueueOK = HIGH;
T0_ms = 1000*Tsampling; // initialize delays to tasks
T1_ms = 5;
T2_ms = 100;
T3_ms = 1000;
T4_ms = 5;
T5_ms = 1000*Tsampling; // initialize delays to tasks
T6_ms = 1000;
T7_ms = 1000;
T0ticks = pdMS_TO_TICKS(T0_ms); // period of Task0 in number of ticks
T1ticks = pdMS_TO_TICKS(T1_ms);
T2ticks = pdMS_TO_TICKS(T2_ms);
T3ticks = pdMS_TO_TICKS(T3_ms);
T4ticks = pdMS_TO_TICKS(T4_ms);
T5ticks = pdMS_TO_TICKS(T5_ms);
T6ticks = pdMS_TO_TICKS(T6_ms);
T7ticks = pdMS_TO_TICKS(T7_ms);
xTaskCreatePinnedToCore(Task0, "Task0", 10000, NULL, 1, &xTask0h, 0);
xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 2, &xTask1h, 1);
xTaskCreatePinnedToCore(Task2, "Task2", 10000, NULL, 2, &xTask2h, 0);
xTaskCreatePinnedToCore(Task3, "Task3", 10000, NULL, 1, &xTask2h, 1);
xTaskCreatePinnedToCore(Task4, "Task4", 10000, NULL, 2, &xTask3h, 1);
xTaskCreatePinnedToCore(Task5, "Task5", 10000, NULL, 1, &xTask5h, 1);
xTaskCreatePinnedToCore(Task6, "Task6", 10000, NULL, 1, &xTask6h, 1);
xTaskCreatePinnedToCore(Task7, "Task7", 10000, NULL, 1, &xTask7h, 1);
}
/**************************************************************************
**************************************************************************/
void Task0( void * parameter)
{
TickType_t xLastWakeTime;
xLastWakeTime = xTaskGetTickCount();
for(;;)
{
SerialDisplay(); //Call serial display
vTaskDelayUntil(&xLastWakeTime, T0ticks); // set period
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task1( void * parameter)
{
for (;;)
{
MotorControl(M0); // Correct Position
vTaskDelay(T1ticks); // set period
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task2( void * parameter)
{
for(;;)
{
velocity = (Step_rev / 35.0); //Set motor velocity controller
vTaskDelay(T2ticks);
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task3( void * parameter)
{
for(;;)
{
// ButtonControl();
vTaskDelay(T3ticks);
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task4( void * parameter)
{
for(;;)
{
MotorControl(M1); // Correct Position
vTaskDelay(T4ticks);
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task5( void * parameter)
{
TickType_t xLastWakeTime;
xLastWakeTime = xTaskGetTickCount();
for(;;)
{
LCDdisplay();
vTaskDelayUntil(&xLastWakeTime, T5ticks); // set period
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task6( void * parameter)
{
for(;;)
{
if(!CMD.CTRL.BIT.Dir)
{
SerialInterface(M0);
}
else
{
SerialInterface(M1);
}
vTaskDelay(T6ticks);
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void Task7( void * parameter)
{
for(;;)
{
if(!CMD.CTRL.BIT.Dir)
{
AdjusGainPID(M0);
}
else
{
AdjusGainPID(M1);
}
vTaskDelay(T7ticks);
}
vTaskDelete(NULL);
}
/**************************************************************************
**************************************************************************/
void setup()
{
Serial.begin(115200);
pinMode(InPin, INPUT); // Set A/D Input Analog Velocity Control
pinMode(KiPin, INPUT); // Set A/D Input Analog Velocity Control
pinMode(KpPin, INPUT); // Set A/D Input Analog Velocity Control
pinMode(KdPin, INPUT); // Set A/D Input Analog Velocity Control
pinMode(SWITCH_ON, INPUT); //Pin D12 SWITCH ON
pinMode(SWITCH_DIR,INPUT); //Pin D13 SWITCH OFF
for(int k = 0; k < NMOTORS; k++)
{
pinMode(enca[k],INPUT);
pinMode(encb[k],INPUT);
pinMode(pwm[k],OUTPUT);
pinMode(in1[k],OUTPUT);
pinMode(in2[k],OUTPUT);
}
freertos_init(); // create tasks and queues here
attachInterrupt(digitalPinToInterrupt(enca[M0]),readEncoder<M0>, FALLING); // update encoder position
attachInterrupt(digitalPinToInterrupt(enca[M1]),readEncoder<M1>, FALLING); // update encoder position
M0PID.SetSampleTime(SampleRate);
M1PID.SetSampleTime(SampleRate);
M0PID.SetMode(AUTOMATIC);
M1PID.SetMode(AUTOMATIC);
M0PID.SetOutputLimits(-255, 255);
M1PID.SetOutputLimits(-255, 255);
delay(1000);
lcd.init(); // initial LCD on lib
lcd.setBacklight(HIGH);
lcd.setCursor(0,0);
lcd.print("PID Motor");
lcd.setCursor(0,1);
lcd.print("Control");
delay(1500);
lcd.clear();
CMD.CTRL.BIT.Run = START; // Enable motor RUN mode
}
/**************************************************************************
**************************************************************************/
void loop()
{
}
/**************************************************************************
**************************************************************************/
void MotorControl(int motor)
{
long currT = micros(); // time difference
float deltaT = ((float) (currT - prevT))/( 1.0e6 ); // Delta T of previous time
prevT = currT; // Set current time to be previous time
SetTarget(currT/1.0e6,deltaT,Motor_dir); // set target position
noInterrupts(); // disable interrupts temporarily while reading
pos[motor] = posi[motor]; // Read the position in an atomic block to avoid a potential misread
interrupts(); // turn interrupts back on
int pwr, dir;
Input[motor] = target[motor]; // target data from step control
Setpoint[motor] = posi[motor]; // modify to fit motor and encoder characteristics, potmeter connected to A0
double gap = abs(Setpoint-Input); //distance away from setpoint
if(motor == M0)
{
if (gap <2)
{
M0PID.SetTunings(consKp, consKi, consKd); //we're close to setpoint, use conservative tuning parameters
}
else
{
M0PID.SetTunings(aggKp, aggKi, aggKd); //we're far from setpoint, use aggressive tuning parameters
}
M0PID.Compute(); // calculate new output
}
if(motor == M1)
{
if (gap < 2)
{
M1PID.SetTunings(consKp, consKi, consKd); //we're close to setpoint, use conservative tuning parameters
}
else
{
M1PID.SetTunings(aggKp, aggKi, aggKd); //we're far from setpoint, use aggressive tuning parameters
}
M1PID.Compute(); // calculate new output
}
pwr = (int) fabs(Output[motor]); // motor power
if( pwr > Umax )
{
pwr = Umax; // Limit maximum pwm
}
else if( pwr < 0 )
{
pwr = 0; // Limit minimum pwm
}
dir = 1; // motor direction positive
if(Output[motor]<0) dir = -1; // motor direction negative
SetMotor(dir, pwr, pwm[motor], in1[motor], in2[motor]); // signal the motor
outputpwm[motor] = pwr; // Output PWM for display
}
/**************************************************************************
**************************************************************************/
void SetTarget(float t, float deltat, int dir)
{
/*
float positionChange[4] = {0.0,0.0,0.0,0.0};
float pulsesPerTurn = 16*18.75;
float pulsesPerMeter = pulsesPerTurn*3.5368;
/***********************************************************/
/*
pulses = 16.0, // 16 pulses / 1 rotation
Turn = 18.75, // 18.75 ratation / 1 output shaft rotation
pulsesPerTurn = pulses*Turn, // 16*18.75 = 300 pulses / Turn
Meter = 3.5368, // circle / wheel size 28 cm (1/0.28)
pulsesPerMeter = pulsesPerTurn*Meter, // 300 * (1/0.28) 3.5368 pulses / Meter
Position change in encoder pulses = {(1 rotation / 1 second ) x Delta T second x (16 * 17.75 pulse / 1 rotation)}
velocity = 0.25; // m/s
*/
/***********************************************************/
t = fmod(t,20); // time is in seconds
if(t < 5) // Wait 4 second
{
dir = Motor_dir;
}
else if(t < 10) // CW Forward 4 second
{
dir = Forward;
}
else if(t < 15) // Wait 4 second
{
dir = Motor_dir;
}
else // CCW Backward 4 second
{
dir = Backward;
}
for(int k = 0; k < 4; k++) // 1 rotation per second
{
positionChange[k] = dir * velocity * deltat * pulsesPerMeter; // Position change in encoder pulses = {(1 rotation / 1 second ) x Delta T second x ( pulse*turn / 1 rotation)}
target_f[k] = target_f[k] + positionChange[k]; // Set target to move robot
}
target[M0] = (long) target_f[M0]; // Wheel 1
target[M1] = (long) - target_f[M1]; // Wheel 2
target[M2] = (long) target_f[M2]; // Wheel 3
target[M3] = (long) - target_f[M3]; // Wheel 4
}
/**************************************************************************
**************************************************************************/
void SetMotor(int dir, int pwmVal, int pwm, int in1, int in2)
{
analogWrite(pwm,pwmVal); //Write analog PWM output
if(dir == CW) // drive motor CW
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
}
else if(dir == CCW) // drive motor CCW
{
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
}
else // Stop motor
{
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
}
}
/**************************************************************************
**************************************************************************/
template <int j>
void readEncoder()
{
int pulse = digitalRead(encb[j]);
if(pulse)
{
posi[j]++;
}
else
{
posi[j]--;
}
}
/**************************************************************************
**************************************************************************/
void SerialDisplay(void)
{
for(int k = 0; k < NMOTORS; k++)
{
Serial.print(target[k]);
Serial.print(" ");
Serial.print(pos[k]);
Serial.print(" ");
}
Serial.println();
}
/**************************************************************************
**************************************************************************/
void LCDdisplay(void)
{
lcd.setCursor(0,0);
lcd.print("M0:"+ String(outputpwm[M0]) + " M1:" + String(outputpwm[M1]));
// lcd.print("Input:"+ String(Input));
lcd.setCursor(0,1);
lcd.print("Velocity:"+ String(velocity) + "m/s");
// lcd.print("Output: " + String(Output));
}
/**************************************************************************
**************************************************************************/
/**************************************************************************
**************************************************************************/
void ButtonControl (void)
{
if(BUTTON_START)
{
long now = millis();
if((now - lastime) > Debounce) // Software debounce
{
lastime = 0;
CMD.CTRL.BIT.Run ^= 1;
}
}
else if(BUTTON_DIR)
{
long now = millis();
if((now - lastime) > Debounce) // Software debounce
{
lastime = 0;
CMD.CTRL.BIT.Dir ^= 1;
}
}
else
{
lastime = 0;
}
}
/**************************************************************************
**************************************************************************/
void SerialInterface(int motor)
{
if (Serial.available()) //**********Inpur Selection****
{
Serial_input_read = Serial.read();
Serial.println(Serial_input_read);
if(!CMD.CTRL.BIT.Run)
{
if (Serial_input_read == '0')
{
Kp[motor] += 0.0001;
Serial.println(Kd[motor]);
}
if (Serial_input_read == '1')
{
Ki[motor] += 0.0001;
Serial.println(Ki[motor]);
}
if (Serial_input_read == '2')
{
Kd[motor] += 0.0001;
Serial.println(Kd[motor]);
}
}
}
}
/**************************************************************************
**************************************************************************/
void AdjusGainPID(int motor)
{
if(!CMD.CTRL.BIT.Run)
{
Kp[motor] = Step_kp;
Ki[motor] = Step_ki;
Kd[motor] = Step_kd;
// pid[motor].setParams(Kp[motor],Ki[motor],Kd[motor],Umax); // A function to set the parameters kp , kd , ki, umaxIn
}
}
/**************************************************************************
**************************************************************************/