|
有需要的可以下载- //利用超声波测距传感器实现小车平衡
- char val='z'; //调节与控制命令字
- double Kp=0.01, Kd=0.02; //PID系数
- unsigned int PB_ad=0, KP_ad=0, KD_ad=0, i; //平衡点调整,PID各调整设定系数
- unsigned int Len_Echo = 0; //回波时间
- int E_0 = 0, E_1 = 0, PWM_Out; //误差,PWM输出
- double Len_filter = 0, Len_0 = 100; //测距滤波,机械平衡距离
- unsigned int TrigPin = 4; //HC-SR04触发信号
- unsigned int EchoPin = 5; //HC-SR04回波检测
- unsigned int M_IN1 = 6; // L298-IN1
- unsigned int M_IN2 = 7; // L298-IN2
- unsigned int M_IN3 = 8; // L298-IN3
- unsigned int M_IN4 = 9; // L298-IN4
- unsigned int M_ENA = 10; // L298-ENA
- unsigned int M_ENB = 11; // L298-ENB
- //初始化
- 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);
- for(i=0; i<64; i++) {
- PB_ad += analogRead(A0); //读取平衡点电位器设定值
- KP_ad += analogRead(A1); //读取PID-P电位器设定值
- KD_ad += analogRead(A2); //读取PID-D电位器设定值
- }
- Kp *= KP_ad/64; //Kp = 3.23;
- Kd *= KD_ad/64; //Kd = 8.14;
- Len_0 += PB_ad/640; //Len_0 = 189;
- i = 0;
- }
- //主循环程序
- void loop() {
- if (Serial.available() > 0) {
- val = Serial.read();
- if(val == 'F') {
- Serial.print(Len_0); Serial.print("\t"); Serial.print(Kp); Serial.print("\t"); Serial.println(Kd);//查看设置参数
- }
- if(val == 'L') {
- Serial.print(Len_Echo); Serial.print("\t"); Serial.println(Len_filter); //查看传感器实测值,滤波值
- }
- }
- digitalWrite(TrigPin, HIGH); //发送超声波测量触发脉冲
- delayMicroseconds(10);
- digitalWrite(TrigPin, LOW);
- Len_Echo = pulseIn(EchoPin, HIGH); //回波时间测量
- if((Len_Echo < 300) && (Len_Echo > 70)) {
- Len_filter *= 0.7; //一阶互补滤波
- Len_filter += 0.3*Len_Echo;
- i ++;
- if(i > 50){
- E_0 = Len_0 - Len_filter;
- PWM_Out = Kp * E_0 + Kd * (E_0 - E_1);
- E_1 = E_0;
- i = 100;
- SetMotorVoltage(PWM_Out);
- }
- }else {
- SetMotorVoltage(0); //超出平衡范围,停止PWM输出
- }
- delay(5); //延时5毫秒,保证超声测距可靠工作
- }
- //电机输出
- void SetMotorVoltage(int MotorVol) {
- if(MotorVol >=0) {
- digitalWrite(M_IN1, LOW);
- digitalWrite(M_IN2, HIGH);
- digitalWrite(M_IN3, LOW);
- digitalWrite(M_IN4, HIGH);
- }else {
- digitalWrite(M_IN1, HIGH);
- digitalWrite(M_IN2, LOW);
- digitalWrite(M_IN3, HIGH);
- digitalWrite(M_IN4, LOW);
- MotorVol = -MotorVol;
- }
- if(MotorVol > 255) MotorVol = 255; //防止PWM值超过255
- analogWrite(M_ENA, MotorVol);
- analogWrite(M_ENB, MotorVol);
- }
复制代码
|
|