|
楼主 |
发表于 2015-4-14 18:53:33
|
显示全部楼层
本帖最后由 wujingyu 于 2015-4-17 10:15 编辑
六、软件设计
软件这里直接上程序吧,很多参数值我并没有给出我调试好的参数,其目的是希望你们不要囫囵吞枣式的干这件事,你们要根据你们自己搭建的小车平台来确定自己程序的参数。整个程序除了前面的宏定义,不到100行,这些程序你们先看,弄明白了,再去调试。
//利用超声波测距传感器实现小车平衡
#define L_max 200
#define L_min 150
#define L_a 0.5
#define filter_init 10
#define L_t 20
byte val = 0x00; //调节与控制命令字
int i, LoopCount = 0; //平衡点调整,PID各调整设定系数
int E_0 = 0, E_1 = 0, PWM_Out, Left_PWM, Right_PWM; //误差,PWM输出,左右电机PWM输出
double Kp = 0.0, Kd = 0.0; //PID系数
double Len_filter = 0, Len_0 = 180; //测距滤波,机械平衡距离
unsigned int TrigPin = 4, EchoPin = 5, Len_Echo = 0; //HC-SR04触发信号,回波检测,回波时间
unsigned int M_IN1 = 6, M_IN2 = 7, M_IN3 = 8, M_IN4 = 9; // L298:IN1-IN4
unsigned int M_ENA = 10, M_ENB = 11; // L298:ENA-ENB
unsigned long systime0; //系统时间
//电机输出
void SetMotorVoltage(int Left_MotorVol, int Right_MotorVol) {
if(Left_MotorVol >= 0) {
digitalWrite(M_IN1, LOW);
digitalWrite(M_IN2, HIGH);
}else {
digitalWrite(M_IN1, HIGH);
digitalWrite(M_IN2, LOW);
Left_MotorVol = -Left_MotorVol;
}
if(Right_MotorVol >= 0) {
digitalWrite(M_IN3, LOW);
digitalWrite(M_IN4, HIGH);
}else {
digitalWrite(M_IN3, HIGH);
digitalWrite(M_IN4, LOW);
Right_MotorVol = -Right_MotorVol;
}
if(Left_MotorVol > 255) Left_MotorVol = 255; //防止PWM值超过255
if(Right_MotorVol > 255) Right_MotorVol = 255; //防止PWM值超过255
analogWrite(M_ENA, Left_MotorVol);
analogWrite(M_ENB, Right_MotorVol);
}
//初始化
void setup() {
Serial.begin(115200);
pinMode(M_ENA, OUTPUT); //电机控制
pinMode(M_IN1, OUTPUT);
pinMode(M_IN2, OUTPUT);
pinMode(M_ENB, OUTPUT);
pinMode(M_IN3, OUTPUT);
pinMode(M_IN4, OUTPUT);
pinMode(EchoPin, INPUT); //超声波测距
pinMode(TrigPin, OUTPUT);
digitalWrite(TrigPin, LOW);
i = 0;
systime0 = millis();
}
//主循环程序
void loop() {
if(millis() - systime0 >= L_t) {
systime0 = millis();
digitalWrite(TrigPin, HIGH); //发送超声波测量触发脉冲
delayMicroseconds(15);
digitalWrite(TrigPin, LOW);
Len_Echo = pulseIn(EchoPin, HIGH); //回波时间测量
if((Len_Echo < L_max) && (Len_Echo > L_min)) {
Len_filter *= L_a; //一阶滤波
Len_filter += (1 - L_a) * Len_Echo;
i ++;
if(i > filter_init){
i = 100;
LoopCount ++;
E_0 = Len_0 - Len_filter;
PWM_Out = Kp * E_0 + Kd * (E_0 - E_1);
E_1 = E_0;
Left_PWM = PWM_Out;
Right_PWM = PWM_Out;
SetMotorVoltage(Left_PWM, Right_PWM);
}
}else {
SetMotorVoltage(0, 0); //超出平衡范围,停止PWM输出
i = 0; Len_filter = 0;
}
}
//处理串口指令,发送相应数据
if (Serial.available() > 0) {
val = Serial.read();
//参数调节
if(val == 0x01) Kp += 0.01;
if(val == 0x02) Kp -= 0.01;
if(val == 0x03) Kd += 0.01;
if(val == 0x04) Kd -= 0.01;
if(val == 0x05) Len_0 ++;
if(val == 0x06) Len_0 --;
if(val == 0x07) {
Serial.print(Len_0); Serial.print("\t");
Serial.print(Kp); Serial.print("\t"); Serial.println(Kd); //查看设置参数
}
if(val == 0x08) {
Serial.print(Len_filter); Serial.print("\t"); Serial.println(PWM_Out); //
}
}
}
源代码下载:
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|