针对马达控制我们主要需要修改两部分:
- 驱动部分,使其可以支持同时控制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驱底盘跑起来。