仅供SDIM大三项目参考使用。
以下是硬件接线图,留意编码器的线序。
Lab1 使用电机编码器测速
首先在Arduino的IDE中安装MsTimer2的库
然后参考以下代码实验,实验内容为:
1、使用电机编码器测速(手指转动电机转子)
2、串口输入电机PWM值,观察电机转速变化
3、使用手指捏住电机转子,观察转速和电流变化
#include <MsTimer2.h> //定时中断头文件库
/***********电机控制板引脚定义************/
unsigned int Motor_AIN1=6; //控制电机的方向引脚 一定改成自己用的
unsigned int Motor_AIN2=7; //控制电机的方向引脚 一定改成自己用的
unsigned int PWML=5; //控制电机的PWM引脚 一定改成自己用的
String Target_Value; //串口获取的速度字符串变量
int value; //用于存储通过PI控制器计算得到的用于调整电机转速的PWM值的整形变量
/***********编码器引脚************/
#define ENCODER_A 2 //编码器A相引脚
#define ENCODER_B 3 //编码器B相引脚
int Velocity,Count=0; //Count计数变量 Velocity存储设定时间内A相上升沿和下降沿的个数
/***********PID控制器相关参数************/
float Velocity_KP =10, Velocity_KI =1;//Velocity_KP,Velocity_KI.PI参数
volatile float Target=0;//目标值
/*********** 限幅************
*以下两个参数让输出的PWM在一个合理区间
*当输出的PWM小于40时电机不转 所以要设置一个启始PWM
*arduino mega 2560 单片机的PWM不能超过255 所以 PWM_Restrict 起到限制上限的作用
*****************************/
int startPWM=0; //初始PWM
int PWM_Restrict=255; //startPW+PWM_Restric=255<256
/***********初始化************/
void setup()
{
Serial.begin(115200); //打开串口
Serial.println("/*****开始驱动*****/");
pinMode(ENCODER_A,INPUT); //设置两个相线为输入模式
pinMode(ENCODER_B,INPUT);
pinMode(Motor_AIN1,OUTPUT); //设置两个驱动引脚为输出模式
pinMode(Motor_AIN2,OUTPUT);
pinMode(PWML,OUTPUT);
MsTimer2::set(50, control); //50毫秒定时中断函数
MsTimer2::start (); //中断使能
attachInterrupt(0, READ_ENCODER_A, FALLING); //开启对应2号引脚的0号外部中断,触发方式为FALLING 即下降沿触发,触发的中断函数为 READ_ENCODER_A
}
/***********主程序************/
void loop()
{
while(Serial.available()>0) //检测串口是否接收到了数据
{
Target_Value=Serial.readString(); //读取串口字符串
Target=Target_Value.toFloat(); //将字符串转换为浮点型,并将其赋给目标值
Serial.print("Target:"); //串口打印出设定的目标转速
Serial.println(Target);
}
Set_PWM((int)Target);
}
/**********外部中断触发计数器函数************
*根据转速的方向不同我们将计数器累计为正值或者负值(计数器累计为正值为负值为计数器方向)
*只有方向累计正确了才可以实现正确的调整,否则会出现逆方向满速旋转
*
*※※※※※※超级重点※※※※※※
*
*所谓累计在正确的方向即
*(1)计数器方向
*(2)电机输出方向(控制电机转速方向的接线是正着接还是反着接)
*(3)PI 控制器 里面的误差(Basi)运算是目标值减当前值(Target-Encoder),还是当前值减目标值(Encoder-Target)
*三个方向只有对应上才会有效果否则你接上就是使劲的朝着一个方向(一般来说是反方向)满速旋转
例子里是已经对应好的,如果其他驱动单片机在自己尝试的时候出现满速旋转就是三个方向没对应上
下列函数中由于在A相上升沿触发时,B相是低电平,和A相下降沿触发时B是高电平是一个方向,在这种触发方式下,我们将count累计为正,另一种情况将count累计为负
********************************************/
void READ_ENCODER_A()
{
if (digitalRead(ENCODER_A) == HIGH)
{
if (digitalRead(ENCODER_B) == LOW)
Count++; //根据另外一相电平判定方向
else
Count--;
}
else
{
if (digitalRead(ENCODER_B) == LOW)
Count--; //根据另外一相电平判定方向
else
Count++;
}
}
/**********定时器中断触发函数*********/
void control()
{
Velocity=Count; //把采用周期(内部定时中断周期)所累计的脉冲下降沿的个数,赋值给速度
Count=0; //将脉冲计数器清零
value=Incremental_PI_A(Velocity,Target); //通过目标值和当前值在这个函数下算出我们需要调整用的PWM值
// Set_PWM(value); //将算好的值输出给电机
}
/***********PI控制器****************/
int Incremental_PI_A (int Encoder,float Target1)
{
static float Bias,PWM=0,Last_bias=0; //定义全局静态浮点型变量 PWM,Bias(本次偏差),Last_bias(上次偏差)
Bias=Target1-Encoder; //计算偏差,目标值减去当前值
PWM += Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制计算
if(PWM>PWM_Restrict)
PWM=PWM_Restrict; //限幅
if(PWM<-PWM_Restrict)
PWM=-PWM_Restrict; //限幅
Last_bias=Bias; //保存上一次偏差
Serial.print((int)Target);
Serial.print(" ");
Serial.println(Encoder);
return PWM; //增量输出
}
/**********PWM控制函数*********/
void Set_PWM(int motora)
{
if (motora > 0) //如果算出的PWM为正
{
digitalWrite(Motor_AIN1, 1);
digitalWrite(Motor_AIN2, 0);
analogWrite(PWML, motora + startPWM); //让PWM在设定正转方向(我们认为的正转方向)正向输出调整
} else if (motora == 0) //如果PWM为0停车
{
digitalWrite(Motor_AIN2, 0);
digitalWrite(Motor_AIN1, 0);
} else if (motora < 0) //如果算出的PWM为负
{
digitalWrite(Motor_AIN1, 0);
digitalWrite(Motor_AIN2, 1);
analogWrite(PWML, -motora + startPWM); //让PWM在设定反转方向反向输出调整
}
}
Lab2 速度闭环控制电机
参考以下代码实验,实验内容为:
1、串口输入(波特率115200)电机目标转速值,观察电机转速变化
2、使用手指捏住电机,观察转速、PWM和电流变化(观察实验电源的电流表)
#include <MsTimer2.h> //定时中断头文件库
/***********电机控制板引脚定义************/
unsigned int Motor_AIN1=6; //控制电机的方向引脚 一定改成自己用的
unsigned int Motor_AIN2=7; //控制电机的方向引脚 一定改成自己用的
unsigned int PWML=5; //控制电机的PWM引脚 一定改成自己用的
String Target_Value; //串口获取的速度字符串变量
int value; //用于存储通过PI控制器计算得到的用于调整电机转速的PWM值的整形变量
/***********编码器引脚************/
#define ENCODER_A 2 //编码器A相引脚
#define ENCODER_B 3 //编码器B相引脚
int Velocity,Count=0; //Count计数变量 Velocity存储设定时间内A相上升沿和下降沿的个数
/***********PID控制器相关参数************/
float Velocity_KP =10, Velocity_KI =1;//Velocity_KP,Velocity_KI.PI参数
volatile float Target=0;//目标值
/*********** 限幅************
*以下两个参数让输出的PWM在一个合理区间
*当输出的PWM小于40时电机不转 所以要设置一个启始PWM
*arduino mega 2560 单片机的PWM不能超过255 所以 PWM_Restrict 起到限制上限的作用
*****************************/
int startPWM=30; //初始PWM
int PWM_Restrict=225; //startPW+PWM_Restric=255<256
/***********初始化************/
void setup()
{
Serial.begin(115200); //打开串口
Serial.println("/*****开始驱动*****/");
pinMode(ENCODER_A,INPUT); //设置两个相线为输入模式
pinMode(ENCODER_B,INPUT);
pinMode(Motor_AIN1,OUTPUT); //设置两个驱动引脚为输出模式
pinMode(Motor_AIN2,OUTPUT);
pinMode(PWML,OUTPUT);
MsTimer2::set(50, control); //50毫秒定时中断函数
MsTimer2::start (); //中断使能
attachInterrupt(0, READ_ENCODER_A, FALLING); //开启对应2号引脚的0号外部中断,触发方式为FALLING 即下降沿触发,触发的中断函数为 READ_ENCODER_A
}
/***********主程序************/
void loop()
{
while(Serial.available()>0) //检测串口是否接收到了数据
{
Target_Value=Serial.readString(); //读取串口字符串
Target=Target_Value.toFloat(); //将字符串转换为浮点型,并将其赋给目标值
Serial.print("Target:"); //串口打印出设定的目标转速
Serial.println(Target);
}
}
/**********外部中断触发计数器函数************
*根据转速的方向不同我们将计数器累计为正值或者负值(计数器累计为正值为负值为计数器方向)
*只有方向累计正确了才可以实现正确的调整,否则会出现逆方向满速旋转
*
*※※※※※※超级重点※※※※※※
*
*所谓累计在正确的方向即
*(1)计数器方向
*(2)电机输出方向(控制电机转速方向的接线是正着接还是反着接)
*(3)PI 控制器 里面的误差(Basi)运算是目标值减当前值(Target-Encoder),还是当前值减目标值(Encoder-Target)
*三个方向只有对应上才会有效果否则你接上就是使劲的朝着一个方向(一般来说是反方向)满速旋转
例子里是已经对应好的,如果其他驱动单片机在自己尝试的时候出现满速旋转就是三个方向没对应上
下列函数中由于在A相上升沿触发时,B相是低电平,和A相下降沿触发时B是高电平是一个方向,在这种触发方式下,我们将count累计为正,另一种情况将count累计为负
********************************************/
void READ_ENCODER_A()
{
if (digitalRead(ENCODER_A) == HIGH)
{
if (digitalRead(ENCODER_B) == LOW)
Count++; //根据另外一相电平判定方向
else
Count--;
}
else
{
if (digitalRead(ENCODER_B) == LOW)
Count--; //根据另外一相电平判定方向
else
Count++;
}
}
/**********定时器中断触发函数*********/
void control()
{
Velocity=Count; //把采用周期(内部定时中断周期)所累计的脉冲下降沿的个数,赋值给速度
Count=0; //将脉冲计数器清零
value=Incremental_PI_A(Velocity,Target); //通过目标值和当前值在这个函数下算出我们需要调整用的PWM值
Set_PWM(value); //将算好的值输出给电机
}
/***********PI控制器****************/
int Incremental_PI_A (int Encoder,float Target1)
{
static float Bias,PWM=0,Last_bias=0; //定义全局静态浮点型变量 PWM,Bias(本次偏差),Last_bias(上次偏差)
Bias=Target1-Encoder; //计算偏差,目标值减去当前值
PWM += Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制计算
if(PWM>PWM_Restrict)
PWM=PWM_Restrict; //限幅
if(PWM<-PWM_Restrict)
PWM=-PWM_Restrict; //限幅
Last_bias=Bias; //保存上一次偏差
Serial.print(PWM);
Serial.print(" ");
Serial.println(Encoder);
return PWM; //增量输出
}
/**********PWM控制函数*********/
void Set_PWM(int motora)
{
if (motora > 0) //如果算出的PWM为正
{
digitalWrite(Motor_AIN1, 1);
digitalWrite(Motor_AIN2, 0);
analogWrite(PWML, motora + startPWM); //让PWM在设定正转方向(我们认为的正转方向)正向输出调整
} else if (motora == 0) //如果PWM为0停车
{
digitalWrite(Motor_AIN2, 0);
digitalWrite(Motor_AIN1, 0);
} else if (motora < 0) //如果算出的PWM为负
{
digitalWrite(Motor_AIN1, 0);
digitalWrite(Motor_AIN2, 1);
analogWrite(PWML, -motora + startPWM); //让PWM在设定反转方向反向输出调整
}
}