坡道行驶小车程序

mym1901500520

energie编写。




#include <Servo.h>
#include <LiquidCrystal.h>

//蜂鸣器端口定义
#define Buzz P1_6 //控制蜂鸣器

//前轮舵机端口定义
Servo carservo;
#define Servopin P2_0//控制舵机

//LCD显示屏端口定义
LiquidCrystal lcd(P8_1, P8_2, P4_3, P4_0, P3_7, P7_4);

//左电机端口定义
#define MotorLpin1   P4_1//控制位3
#define MotorLpin2   P4_2//控制位4
#define MotorLpwm    P3_5 //使能调速 ENB
#define MotorLcountA P1_3 //编码器B

//右电机端口定义
#define MotorRpin1   P2_7//控制位1
#define MotorRpin2   P3_2 //控制位2
#define MotorRpwm    P3_6  //使能调速 ENA
#define MotorRcountA P1_2 //编码器B

//四路传感器
#define sensor1 P6_0
#define sensor2 P6_1
#define sensor3 P6_2

//控速变量
volatile float motorL=0;//中断变量，左轮子脉冲计数
volatile float motorR=0;//中断变量，右轮子脉冲计数
float V_L=0; //左轮速度 单位cm/s
float V_R=0; //右边轮速 单位cm/s
int v1=0;  //单位cm/s
int v2=0;  //单位cm/s
float Target_V_L=8,Target_V_R=8;   //目标速度，单位cm/s
int Pwm_L=0,Pwm_R=0;  //左右轮PWM

//PID变量
float kp=1.6,ki=0.8,kd=0;  //PID参数


/**************************************
* 初始化函数
*************************************/
void setup() {
   //LCD初始化
   lcd.begin(16, 2);
   lcd.print("Start");
   Motor_Init();//电机端口初始化
   Servo_Init();//舵机端口初始化
   pinMode(Buzz,OUTPUT);//蜂鸣器初始化
   //循迹初始化
   pinMode(sensor1,INPUT);
   pinMode(sensor2,INPUT);
   pinMode(sensor3,INPUT);
   Serial.begin(9600);//开启串口
}

void Servo_Init(){
   carservo.attach(Servopin);
   carservo.write(82);
   delay(15);
}

void Motor_Init(){
  //左电机
  pinMode(MotorLpin1,OUTPUT);  //驱动芯片控制引脚
  pinMode(MotorLpin2,OUTPUT);  //驱动芯片控制引脚
  pinMode(MotorLpwm,OUTPUT);   //驱动芯片控制引脚，PWM调速
  pinMode(MotorLcountA,INPUT); //左轮编码器B引脚
  
  //右电机
  pinMode(MotorRpin1,OUTPUT);  //驱动芯片控制引脚
  pinMode(MotorRpin2,OUTPUT);  //驱动芯片控制引脚
  pinMode(MotorRpwm,OUTPUT);   //驱动芯片控制引脚，PWM调速
  pinMode(MotorRcountA,INPUT); //右轮编码器B引脚

  //驱动芯片控制引脚全部拉低
  digitalWrite(MotorLpin1,LOW); //左电机
  digitalWrite(MotorLpin2,LOW);
  digitalWrite(MotorLpwm,LOW);
  digitalWrite(MotorRpin1,LOW); //右电机
  digitalWrite(MotorRpin2,LOW);
  digitalWrite(MotorRpwm,LOW);
}


/*********************************************************
* 函数功能：增量式PI控制器(左轮)
*********************************************************/
int Incremental_Pi_L(int current_speed,int target_speed){
  static float pwm,bias,last_bias,prev_bias;  //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
  bias=current_speed-target_speed;    //计算本次偏差e(k)
  pwm-=(kp*(bias-last_bias)+ki*bias+kd*(bias-2*last_bias+prev_bias));   //增量式PID控制器
  prev_bias=last_bias;  //保存上上次偏差
  last_bias=bias;     //保存上一次偏差
  //PWM 限幅度  Arduino的PWM 最高为255  限制在250
  if(pwm<-250){
    pwm=250;     
  }
  if(pwm>250){
    pwm=250;  
  }
  //Serial.println(pwm);
  return pwm;         //增量输出
}

//右轮速度增量式PID控制器
int Incremental_Pi_R(float current_speed,float target_speed){
  static float pwm,bias,last_bias,prev_bias;  //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
  bias=current_speed-target_speed;    //计算本次偏差e(k)
  pwm-=(kp*(bias-last_bias)+ki*bias+kd*(bias-2*last_bias+prev_bias));   //增量式PID控制器
  prev_bias=last_bias;  //保存上上次偏差
  last_bias=bias;     //保存上一次偏差

  //PWM 限幅度  Arduino的PWM 最高为255限制在250
  if(pwm<-250){
    pwm=250;     
  }
  if(pwm>250){
    pwm=250;  
  }
  //Serial.println(pwm);
  return pwm;         //增量输出
}

void Set_Pwm(int speed_L,int speed_R){
  //前进模式
  //左电机
  digitalWrite(MotorLpin1,HIGH);
  digitalWrite(MotorLpin2,LOW);
  analogWrite(MotorLpwm,speed_L);
  //右电机
  digitalWrite(MotorRpin1,LOW);
  digitalWrite(MotorRpin2,HIGH);
  analogWrite(MotorRpwm,speed_R);
// Serial.println("lello");
}


/***********************************
* 电机实际速度计算：
* 公式：
* 已知参数：
*     车轮直径65mm,
*     左边轮子一圈：390脉冲（RISING）,
*     右边轮子一圈：390脉冲（RISING），
* 单位时间读两个轮子脉冲读取两个轮子脉冲
***********************************/
void Read_Moto_V(){
  unsigned long nowtime=0;
  motorL=0;
  motorR=0;
  nowtime=millis()+50;//读50毫秒
  attachInterrupt(digitalPinToInterrupt(MotorLcountA),Read_Moto_L,RISING);//左轮脉冲开中断计数
  attachInterrupt(digitalPinToInterrupt(MotorRcountA),Read_Moto_R,RISING);//右轮脉冲开中断计数
  while(millis()<nowtime); //达到50毫秒关闭中断
  detachInterrupt(digitalPinToInterrupt(MotorLcountA));//左轮脉冲关中断计数
  detachInterrupt(digitalPinToInterrupt(MotorRcountA));//右轮脉冲关中断计数
  //Serial.println(motorL*100);
  //Serial.println(motorR*100);
  V_L=((motorL/390)*6.5*PI)/0.05;   //单位cm/s
  V_R=((motorR/390)*6.5*PI)/0.05;   //单位cm/s
  v1=V_L;
  v2=V_R;
}


/***************************
* 中断函数：读左轮脉冲
**************************/
void Read_Moto_L(){
  motorL++;
}


/**************************
* 中断函数：读右轮脉冲
*************************/
void Read_Moto_R(){
  motorR++;
}

//前进函数
void RUN(int RV,int LV){
   Target_V_L=LV,Target_V_R=RV;
   Read_Moto_V();//读取脉冲计算速度
   Pwm_L=Incremental_Pi_L(V_L,Target_V_L);//左轮PI运算
   Pwm_R=Incremental_Pi_R(V_R,Target_V_R);//右轮PI运算
   //Serial.println(V_L);  //直接用串口绘图画出速度曲线
   Set_Pwm(Pwm_L,Pwm_R);  //设置左右轮速度
}



//终点停车函数
void Stop(){
   //左电机
   digitalWrite(MotorLpin1,LOW);
   digitalWrite(MotorLpin2,LOW);
   //右电机
   digitalWrite(MotorRpin1,LOW);
   digitalWrite(MotorRpin2,LOW);
   //声音提示
   digitalWrite(Buzz,HIGH);
   delay(500);
   digitalWrite(Buzz,LOW);
   delay(1000);
   while(1);
}

void LCDshow(){
   lcd.setCursor(0, 0);
   lcd.print("Speed:");
   lcd.setCursor(6, 0);
   lcd.print(V_L);
   lcd.setCursor(11, 0);
   lcd.print("cm/s");
   lcd.setCursor(0, 1);
   lcd.print("Time:");
   lcd.setCursor(5, 1);
   lcd.print(millis()*0.001);
   lcd.setCursor(10, 1);
   lcd.print("s");
}


void Track(){
   int data[3];
   data[0] = analogRead(sensor1);//读取四个传感器值
   data[0] = map(data[0], 0, 4095, 0, 1000);
   data[1] = analogRead(sensor2);
   data[1] = map(data[1], 0, 4095, 0, 1000);
   data[2] = analogRead(sensor3);
   data[2] = map(data[2], 0, 4095, 0, 1000);
   
   if(data[0]<200&&data[1]>200&&data[2]<200){
       carservo.write(82);
       delay(15);
       RUN(6,6);
    }
   else if((data[0]<200&&data[1]<200&&data[2]<200)&&carservo.read()<100&&carservo.read()>70){
       RUN(6,6);
    }
   else if((data[0]<200&&data[1]<200&&data[2]<200)&&carservo.read()>100){
       RUN(6,10);
    }
   else if(data[0]>200&&data[1]<200&&data[2]<200)
       Turn_right();
   else if(data[0]<200&&data[1]<200&&data[2]>200)
       Turn_left();
   else if(data[0]>200&&data[1]>200&&data[2]>200)
       Stop();
   /*Serial.println(data[0]);
   Serial.println(data[1]);
   Serial.println(data[2]);
   Serial.println("--------------");*/
}
//右转函数
void Turn_right(){
  carservo.write(140);//调整舵机角度 140 9.8 2.2
  delay(15);
  RUN(6,10);//后轮差速（右，左），左快右慢
}

//左转函数
void Turn_left(){
  carservo.write(60);//调整舵机角度
  delay(15);
  RUN(10,6);//后轮差速（右，左），左慢右快
}

/***************************************
* 主循环
***************************************/
void loop() {
   LCDshow();
   Track();
}

bluesky

谢谢分享

hanqii

学到了！