Programa
demo_2wd_pid_steering_speed
#define serialSpeed 115200
#define bodyLeftMotorPwmBase 0
#define bodyRightMotorPwmBase 20
#define bodySteeringFactor 1
// encoder type
#define bodyEncoderSJ01 true
#define bodyEncoderHC020K false
#define bodyEncoderLM393 false
#if bodyEncoderSJ01
#define bodyEncoderSignal CHANGE
#define bodyEncoderInputSignal INPUT
#endif
#if bodyEncoderHC020K
#define bodyEncoderSignal FALLING
#define bodyEncoderInputSignal INPUT_PULLUP
#endif
#if bodyEncoderLM393
#define bodyEncoderSignal RISING
#define bodyEncoderInputSignal INPUT
#endif
// 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
#if bodyEncoderSJ01
#define bodyEncoderLeftPinB 9 // B pin: the digital pin
#endif
#define bodyEncoderLeftSignal bodyEncoderSignal
#define bodyEncoderLeftInputSignal bodyEncoderInputSignal
// encoder RightMotor
#define bodyEncoderRightInt 1
#define bodyEncoderRightFunction bodyEncoderRightCounter
#define bodyEncoderRightPin 3 // A pin: the interrupt pin
#if bodyEncoderSJ01
#define bodyEncoderRightPinB 10 // B pin: the digital pin
#endif
#define bodyEncoderRightSignal bodyEncoderSignal
#define bodyEncoderRightInputSignal bodyEncoderInputSignal
// steering PID
#define bodySteeringPidSetPointTarget 0
#define bodySteeringPidSampleTime 5
#define bodySteeringPidMinOutput -100
#define bodySteeringPidMaxOutput 100
// speed PID
#define bodySpeedPidSetPointTarget 10
#define bodySpeedPidSampleTime 25
#define bodySpeedPidMinOutput 50
#define bodySpeedPidMaxOutput 200
#include <PID_v1.h>
#if bodyEncoderSJ01
byte bodyEncoderLeftPinLast;
boolean bodyEncoderLeftDirection; // the rotation direction
byte bodyEncoderRightPinLast;
boolean bodyEncoderRightDirection; //the rotation direction
#endif
volatile signed long bodyEncoderLeftPulses = 0;
volatile signed long bodyEncoderRightPulses = 0;
volatile signed long bodyEncoderLeftTotalPulses = 0;
volatile signed long bodyEncoderRightTotalPulses = 0;
// steering PID
double bodySteeringPidInput;
double bodySteeringPidOutput; // Power supplied to the motor PWM value.
double bodySteeringPidSetPoint;
double bodySteeringPidKp=1, bbodySteeringPidKi=0.4, bodySteeringPidKd=0.001;
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);
// speed PID
bodySpeedPidSetPoint = bodySpeedPidSetPointTarget; // speed in pulses per sample time
bodySpeedPid.SetMode(AUTOMATIC);
bodySpeedPid.SetSampleTime(bodySpeedPidSampleTime);
bodySpeedPid.SetOutputLimits(bodySpeedPidMinOutput, bodySpeedPidMaxOutput);
// encoders
#if bodyEncoderSJ01
bodyEncoderLeftDirection = true; //default -> Forward
pinMode(bodyEncoderLeftPinB, bodyEncoderLeftInputSignal);
#endif
attachInterrupt(bodyEncoderLeftInt, bodyEncoderLeftFunction, bodyEncoderLeftSignal);
#if bodyEncoderSJ01
bodyEncoderRightDirection = true; // default -> Forward
pinMode(bodyEncoderRightPinB, bodyEncoderRightInputSignal);
#endif
attachInterrupt(bodyEncoderRightInt, bodyEncoderRightFunction, bodyEncoderRightSignal);
delay(3000);
loopTimeStart = millis();
}
void loop() {
unsigned long loopTime = 0;
unsigned int bodyLeftMotorPwmOut = 0;
unsigned int bodyRightMotorPwmOut = 0;
signed long bodySteeringError = 0;
boolean bodySteeringdPidResult = false;
signed long bodySpeedOnSample = 0;
boolean bodySpeedPidResult = false;
// set direction
setBodyMotorLeftForward();
setBodyMotorRightForward();
// calculate pwm base for desired speed
bodySpeedOnSample = (abs(bodyEncoderLeftPulses) + abs(bodyEncoderRightPulses)) / 2;
bodySpeedPidInput = bodySpeedOnSample;
bodySpeedPidResult = bodySpeedPid.Compute();
if(bodySpeedPidResult) {
bodyEncoderLeftPulses = 0;
bodyEncoderRightPulses = 0;
}
// calculate pwm wheel diference correction using pid
bodySteeringError = abs(bodyEncoderLeftTotalPulses) - abs(bodyEncoderRightTotalPulses);
bodySteeringPidInput = bodySteeringError;
bodySteeringPid.Compute();
bodyLeftMotorPwmOut = bodySpeedPidOutput - (bodySteeringPidOutput * bodySteeringFactor);
bodyRightMotorPwmOut = bodySpeedPidOutput + (bodySteeringPidOutput * bodySteeringFactor);
// set speed
analogWrite(bodyLeftMotorEn, bodyLeftMotorPwmOut);
analogWrite(bodyRightMotorEn, bodyRightMotorPwmOut);
if(bodySpeedPidResult) {
// display info
Serial.print(bodySteeringError); Serial.print("\t");
//Serial.print(abs(bodyEncoderLeftPulses)); Serial.print("\t");
//Serial.print(abs(bodyEncoderRightPulses)); Serial.print("\t");
Serial.print(bodySpeedPidSetPoint - bodySpeedOnSample); Serial.print("\t");
Serial.println("");
}
// end loop
loopTime = millis() - loopTimeStart;
loopTimeStart = millis();
}
void bodyEncoderLeftCounter()
{
#if bodyEncoderSJ01
int Lstate = digitalRead(bodyEncoderLeftPin);
if((bodyEncoderLeftPinLast == LOW) && Lstate==HIGH)
{
int val = digitalRead(bodyEncoderLeftPinB);
if(val == LOW && bodyEncoderLeftDirection)
{
bodyEncoderLeftDirection = false; //Reverse
}
else if(val == HIGH && !bodyEncoderLeftDirection)
{
bodyEncoderLeftDirection = true; //Forward
}
}
bodyEncoderLeftPinLast = Lstate;
if(!bodyEncoderLeftDirection)
{
bodyEncoderLeftPulses ++;
bodyEncoderLeftTotalPulses++;
} else {
bodyEncoderLeftPulses --;
bodyEncoderLeftTotalPulses--;
}
#else
bodyEncoderLeftPulses ++;
bodyEncoderLeftTotalPulses ++;
#endif
}
void bodyEncoderRightCounter()
{
#if bodyEncoderSJ01
int Lstate = digitalRead(bodyEncoderRightPin);
if((bodyEncoderRightPinLast == LOW) && Lstate==HIGH)
{
int val = digitalRead(bodyEncoderRightPinB);
if(val == LOW && bodyEncoderRightDirection)
{
bodyEncoderRightDirection = false; // Reverse
}
else if(val == HIGH && !bodyEncoderRightDirection)
{
bodyEncoderRightDirection = true; // Forward
}
}
bodyEncoderRightPinLast = Lstate;
if(!bodyEncoderRightDirection)
{
bodyEncoderRightPulses ++;
bodyEncoderRightTotalPulses ++;
} else {
bodyEncoderRightPulses --;
bodyEncoderRightTotalPulses ++;
}
#else
bodyEncoderRightPulses ++;
bodyEncoderRightTotalPulses ++;
#endif
}
void setBodyMotorLeftForward() {
digitalWrite(bodyLeftMotorForward, HIGH);
digitalWrite(bodyLeftMotorBackward, LOW);
}
void setBodyMotorRightForward() {
digitalWrite(bodyRightMotorForward, HIGH);
digitalWrite(bodyRightMotorBackward, LOW);
}
end