#define serialSpeed 115200
#define bodyLeftMotorPwmBase 0
#define bodyRightMotorPwmBase 0
#define bodySteeringFactor 1
#define bodyEncoderSignal FALLING
#define bodyEncoderInputSignal INPUT_PULLUP
// motor one
#define enA 5
#define in1 4
#define in2 7
// motor two
#define enB 6
#define in3 8
#define in4 12
// LeftMotor
#define bodyLeftMotorEn enA
#define bodyLeftMotorForward in1
#define bodyLeftMotorBackward in2
// RightMotor
#define bodyRightMotorEn enB
#define bodyRightMotorForward in4
#define bodyRightMotorBackward in3
// encoder LeftMotor
#define bodyEncoderLeftInt 0
#define bodyEncoderLeftFunction bodyEncoderLeftCounter
#define bodyEncoderLeftPin 2 // A pin the interrupt pin
#define bodyEncoderLeftSignal bodyEncoderSignal
#define bodyEncoderLeftInputSignal bodyEncoderInputSignal
// encoder RightMotor
#define bodyEncoderRightInt 1
#define bodyEncoderRightFunction bodyEncoderRightCounter
#define bodyEncoderRightPin 3 // A pin: the interrupt pin
#define bodyEncoderRightSignal bodyEncoderSignal
#define bodyEncoderRightInputSignal bodyEncoderInputSignal
// steering PID
#define bodySteeringPidSetPointTarget 0
#define bodySteeringPidSampleTime 5
#define bodySteeringPidMinOutput -20
#define bodySteeringPidMaxOutput 20
#define bodySteeringPidKp1 5
#define bodySteeringPidKi1 1
#define bodySteeringPidKd1 0
#define bodySteeringPidKp0 0.5
#define bodySteeringPidKi0 0.1
#define bodySteeringPidKd0 0.001
#define bodySteeringPidAdaptativeDelay 300
#define bodySteeringPidAdaptativeLimit 3
boolean bodySteeringPidAdaptativeStart = false;
unsigned long bodySteeringPidAdaptativeTimer;
// speed PID
#define bodySpeedPidSetPointTarget 2
#define bodySpeedPidSampleTime 100
#define bodySpeedPidMinOutput 80
#define bodySpeedPidMaxOutput 200
#include <PID_v1.h>
volatile long bodyEncoderLeftPulses = 0;
volatile long bodyEncoderRightPulses = 0;
volatile long bodyEncoderLeftTotalPulses = 0;
volatile long bodyEncoderRightTotalPulses = 0;
// steering PID
double bodySteeringPidInput;
double bodySteeringPidOutput; // Power supplied to the motor PWM value.
double bodySteeringPidSetPoint;
double bodySteeringPidKp=bodySteeringPidKp1, bbodySteeringPidKi=bodySteeringPidKi1, bodySteeringPidKd=bodySteeringPidKd1;
PID bodySteeringPid(&bodySteeringPidInput, &bodySteeringPidOutput, &bodySteeringPidSetPoint, bodySteeringPidKp, bbodySteeringPidKi, bodySteeringPidKd, REVERSE);
// speed PID
double bodySpeedPidInput;
double bodySpeedPidOutput; // Power supplied to the motor PWM value.
double bodySpeedPidSetPoint;
double bodySpeedPidKp=0.6, bbodySpeedPidKi=5, bodySpeedPidKd=0;
PID bodySpeedPid(&bodySpeedPidInput, &bodySpeedPidOutput, &bodySpeedPidSetPoint, bodySpeedPidKp, bbodySpeedPidKi, bodySpeedPidKd, DIRECT);
unsigned long loopTimeStart = 0;
void setup() {
Serial.begin(serialSpeed); //Initialize the serial port
// bridge-H
pinMode(bodyLeftMotorEn, OUTPUT);
pinMode(bodyLeftMotorForward, OUTPUT);
pinMode(bodyLeftMotorBackward, OUTPUT);
pinMode(bodyRightMotorEn, OUTPUT);
pinMode(bodyRightMotorForward, OUTPUT);
pinMode(bodyRightMotorBackward, OUTPUT);
// steering PID
bodySteeringPidSetPoint = bodySteeringPidSetPointTarget;
bodySteeringPid.SetMode(AUTOMATIC);
bodySteeringPid.SetSampleTime(bodySteeringPidSampleTime);
bodySteeringPid.SetOutputLimits(bodySteeringPidMinOutput, bodySteeringPidMaxOutput);
bodySteeringPid.SetTunings(bodySteeringPidKp1, bodySteeringPidKi1, bodySteeringPidKd1);
bodySteeringPidAdaptativeStart = true;
bodySteeringPidAdaptativeTimer = millis() + bodySteeringPidAdaptativeDelay;
// speed PID
bodySpeedPidSetPoint = bodySpeedPidSetPointTarget; // speed in pulses per sample time
bodySpeedPid.SetMode(AUTOMATIC);
bodySpeedPid.SetSampleTime(bodySpeedPidSampleTime);
bodySpeedPid.SetOutputLimits(bodySpeedPidMinOutput, bodySpeedPidMaxOutput);
// encoders
attachInterrupt(bodyEncoderLeftInt, bodyEncoderLeftFunction, bodyEncoderLeftSignal);
attachInterrupt(bodyEncoderRightInt, bodyEncoderRightFunction, bodyEncoderRightSignal);
delay(3000);
loopTimeStart = millis();
}
void loop() {
unsigned long loopTime = 0;
unsigned int bodyLeftMotorPwmOut = 0;
unsigned int bodyRightMotorPwmOut = 0;
int bodySteeringError = 0;
boolean bodySteeringdPidResult = false;
boolean bodySpeedPidResult = false;
// set direction
setBodyMotorLeftForward();
setBodyMotorRightForward();
// calculate pwm base for desired speed
bodySpeedPidInput = (abs(bodyEncoderLeftPulses) + abs(bodyEncoderRightPulses)) / 2;
bodySpeedPidResult = bodySpeedPid.Compute();
if(bodySpeedPidResult) {
bodyEncoderLeftPulses = 0;
bodyEncoderRightPulses = 0;
}
// calculate pwm wheel diference correction using pid
bodySteeringPidInput = abs(bodyEncoderLeftTotalPulses) - abs(bodyEncoderRightTotalPulses);
// adaptative steering PID
if(!bodySteeringPidAdaptativeStart) {
if(bodySteeringPidInput > bodySteeringPidAdaptativeLimit) {
bodySteeringPid.SetTunings(bodySteeringPidKp1, bodySteeringPidKi1, bodySteeringPidKd1);
} else {
bodySteeringPid.SetTunings(bodySteeringPidKp0, bodySteeringPidKi0, bodySteeringPidKd0);
}
} else {
if(millis() > bodySteeringPidAdaptativeTimer) {
bodySteeringPidAdaptativeStart = true;
}
}
bodySteeringPid.Compute();
bodyLeftMotorPwmOut = bodySpeedPidOutput - (bodySteeringPidOutput * bodySteeringFactor);
bodyRightMotorPwmOut = bodySpeedPidOutput + (bodySteeringPidOutput * bodySteeringFactor);
// set speed
analogWrite(bodyLeftMotorEn, bodyLeftMotorPwmOut);
analogWrite(bodyRightMotorEn, bodyRightMotorPwmOut);
if(bodySpeedPidResult) {
// display info
Serial.print(bodySpeedPidOutput/10); Serial.print("\t");
Serial.print(bodySteeringPidOutput/10); Serial.print("\t");
Serial.print(bodySteeringPidInput); Serial.print("\t");
//Serial.print(abs(bodyEncoderLeftPulses)); Serial.print("\t");
//Serial.print(abs(bodyEncoderRightPulses)); Serial.print("\t");
Serial.print(bodySpeedPidSetPoint - bodySpeedPidInput); Serial.print("\t");
Serial.println("");
}
// end loop
loopTime = millis() - loopTimeStart;
loopTimeStart = millis();
}
void bodyEncoderLeftCounter() {
bodyEncoderLeftPulses ++;
bodyEncoderLeftTotalPulses ++;
}
void bodyEncoderRightCounter() {
bodyEncoderRightPulses ++;
bodyEncoderRightTotalPulses ++;
}
void setBodyMotorLeftForward() {
digitalWrite(bodyLeftMotorForward, HIGH);
digitalWrite(bodyLeftMotorBackward, LOW);
}
void setBodyMotorRightForward() {
digitalWrite(bodyRightMotorForward, HIGH);
digitalWrite(bodyRightMotorBackward, LOW);
}