更多创客作品,请关注笔者网站园丁鸟,搜集全球极具创意,且有价值的创客作品
针对马达控制我们主要需要修改两部分:
驱动部分,使其可以支持同时控制4个马达
PID调速部分,试验表明,虽然是一样的型号的马达,但是给相同的PWM值,马达的转速也不一样,所以我们需要分别对4个马达进行PID调速
1.马达驱动
1.1. DualL298PMotorShield4WD.h文件的修改
增加了4个马达对应的PWM和方向控制的Pin引脚定义,和4个马达速度的设置函数
#ifndef DualL298PMotorShield4WD_h
#define DualL298PMotorShield4WD_h
#include
class DualL298PMotorShield4WD
{
public:
// CONSTRUCTORS
DualL298PMotorShield4WD(); // Default pin selection.
// PUBLIC METHODS
void init(); // Initialize TIMER 1, set the PWM to 20kHZ.
void setM1Speed(int speed); // Set speed for M1.
void setM2Speed(int speed); // Set speed for M2.
void setM3Speed(int speed); // Set speed for M3.
void setM4Speed(int speed); // Set speed for M4.
void setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed); // Set speed for both M1 and M2.
private:
// left motor
static const unsigned char _M1DIR = 12;
static const unsigned char _M2DIR = 7;
static const unsigned char _M1PWM = 10;
static const unsigned char _M2PWM = 6;
// right motor
static const unsigned char _M4DIR = 8;
static const unsigned char _M3DIR = 13;
static const unsigned char _M4PWM = 9;
static const unsigned char _M3PWM = 11;
};
#endif
1.2. DualL298PMotorShield4WD.cpp文件的修改
主要是针对4个马达速度设置函数的实现,逻辑都是一样的,这里只截取一个马达的代码,代码的逻辑请看代码注释
// Set speed for motor 4, speed is a number betwenn -400 and 400
void DualL298PMotorShield4WD::setM4Speed(int speed)
{
unsigned char reverse = 0;
if (speed < 0) //速度是否小于0 { speed = -speed; // 如果小于0,则是反转 reverse = 1; // 反转标志变量设置为1 } if (speed > 255) // 限定最大速度为255
speed = 255;
if (reverse) //反转状态下
{
digitalWrite(_M4DIR,LOW);//设定马达转向的pin位低电平
analogWrite(_M4PWM, speed);
}
else //正向转动状态下
{
digitalWrite(_M4DIR,HIGH);设定马达转向的pin位高电平
analogWrite(_M4PWM, speed);
}
}
在setSpeeds函数中分别调用4个马达的速度设置函数。
// Set speed for motor 1, 2, 3, 4
void DualL298PMotorShield4WD::setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed)
{
setM1Speed(m1Speed);
setM2Speed(m2Speed);
setM3Speed(m3Speed);
setM4Speed(m4Speed);
}
DualL298PMotorShield4WD.h和DualL298PMotorShield4WD.cpp修改完成后,在arduino 的library目录下新建一个名为dual-L298P-motor-shield-master-4wd的目录,将两个文件放到此文件夹下
现在我们打开android ide的library就可以看到我们刚才添加的库
1.1.3 motor_driver.h的修改
增加4驱马达控制的函数setMotorSpeeds,参数为4个马达的速度
void initMotorController();
void setMotorSpeed(int i, int spd);
#ifdef L298P
void setMotorSpeeds(int leftSpeed, int rightSpeed);
#endif
#ifdef L298P_4WD
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2);
#endif
1.1.4 motor_driver.ino的修改
#ifdef L298P_4WD
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2){
setMotorSpeed(1, leftSpeed_1);
setMotorSpeed(3, rightSpeed_1);
setMotorSpeed(2, leftSpeed_2);
setMotorSpeed(4, rightSpeed_2);
}
#endif
#else
#error A motor driver must be selected!
#endif
2.PID控制
PID控制都在diff_controller.h文件中定义修改
新增两个在4驱模式下的PID控制变量
#ifdef L298P_4WD
SetPointInfo leftPID_h, rightPID_h;
#endif
新增两个在4驱模式下的PID控制参数
#ifdef L298P_4WD
int left_h_Kp=Kp;
int left_h_Kd=Kd;
int left_h_Ki=Ki;
int left_h_Ko=Ko;
int right_h_Kp=Kp;
int right_h_Kd=Kd;
int right_h_Ki=Ki;
int right_h_Ko=Ko;
#endif
新增两个在4驱模式下的PID控制函数
#ifdef L298P_4WD
/* PID routine to compute the next motor commands */
void dorightPID_h(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder-p->PrevEnc ;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (right_h_Kp * Perror - right_h_Kd * (input - p->PrevInput) + p->ITerm) / right_h_Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;
p->output = output;
p->PrevInput = input;
// Serial.println("right output:");
// Serial.println(p->output);
}
#endif
...
#ifdef L298P_4WD
/* PID routine to compute the next motor commands */
void doleftPID_h(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder-p->PrevEnc ;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (left_h_Kp * Perror - left_h_Kd * (input - p->PrevInput) + p->ITerm) / left_h_Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;
p->output = output;
p->PrevInput = input;
// Serial.println("left output:");
// Serial.println(p->output);
}
#endif
修改readPIDIn函数,增加在4驱模式下,pidin的读取
long readPidIn(int i) {
long pidin=0;
if (i == LEFT){
pidin = leftPID.PrevInput;
}else if (i == RIGHT){
pidin = rightPID.PrevInput;
}
#ifdef L298P_4WD
else if (i== RIGHT_H){
pidin = rightPID_h.PrevInput;
}else{
pidin = leftPID_h.PrevInput;
}
#endif
return pidin;
}
修改readPIDOut函数,增加在4驱模式下,pidOut的读取
long readPidOut(int i) {
long pidout=0;
if (i == LEFT){
pidout = leftPID.output;
}else if (i == RIGHT){
pidout = rightPID.output;
}
#ifdef L298P_4WD
else if (i == RIGHT_H){
pidout = rightPID_h.output;
}else{
pidout = leftPID_h.output;
}
#endif
return pidout;
}
修改updatePID函数,增加在4驱模式下马达PID控制的调用
void updatePID() {
/* Read the encoders */
leftPID.Encoder =readEncoder(LEFT);
rightPID.Encoder =readEncoder(RIGHT);
#ifdef L298P_4WD
leftPID_h.Encoder =readEncoder(LEFT_H);
rightPID_h.Encoder =readEncoder(RIGHT_H);
#endif
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
#ifdef L298P
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
#endif
#ifdef L298P_4WD
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0 || leftPID_h.PrevInput != 0 || rightPID_h.PrevInput != 0) resetPID();
#endif
return;
}
/* Compute PID update for each motor */
dorightPID(&rightPID);
doleftPID(&leftPID);
#ifdef L298P_4WD
dorightPID_h(&rightPID_h);
doleftPID_h(&leftPID_h);
#endif
/* Set the motor speeds accordingly */
#ifdef L298P
setMotorSpeeds(leftPID.output, rightPID.output);
#endif
#ifdef L298P_4WD
setMotorSpeeds(leftPID.output,leftPID_h.output, rightPID.output,rightPID_h.output);
#endif
}
至此4个马达已经可以独立驱动,独立的PID调速了,在下一篇教程中,会介绍如和上位机互动起来,让4驱底盘跑起来。