32单片机自主移动小车代码

Question

小车代码已经可以实现点到点的移动，在中间多加几个点，实现小车在移动到终点的过程中，经过特定点的功能,不知道该怎么加，stm32。

#include "control.h"    
#define T 0.156f
#define L 0.1445f
#define K 311.4f
u8 Flag_Target;
int Voltage_Temp,Voltage_Count,Voltage_All,sum;
/**************************************************************************
//函数功能：小车运动数学模型
//入口参数：速度和转角
//返回  值：无
**************************************************************************/
void Kinematic_Analysis(float velocity,float turn)
{
      Target_A=(velocity+turn); 
        Target_B=(velocity-turn);      //后轮差速
}


void SelfLocalization(void)    //小车自定位
{
    const double T_Sampling=0.005*2;    //5ms*2    采样周期
    const double Encoder_Round=390*4;    //编码器线数*4倍频细分
    const double R=0.032, B=0.155; //车轮半径，轮距，m
    
    //直接可用的全局变量
    //extern int Encoder_Left,Encoder_Right; //左右编码器的脉冲计数
    //extern float X_me,Y_me,theta_me;    //小车位置和姿态
    //extern float V_me,W_me;    //小车线速度和角速度-编码器估计值    
    
    //补全如下代码，计算小车线速度和角速度，以及小车位置和姿态
    double wL,wR;
    wL=Encoder_Left/Encoder_Round*2*PI/T_Sampling;//左轮转速
    wR=Encoder_Right/Encoder_Round*2*PI/T_Sampling;//右轮转速
    

    V_me=(R*wL+R*wR)/2;
    W_me=(R*wR-R*wL)/B;
//    

//    V_me=?;
//    W_me=?;
//    
    X_me=X_me+V_me*T_Sampling*cos(theta_me+W_me*T_Sampling/2);
    Y_me=Y_me+V_me*T_Sampling*sin(theta_me+W_me*T_Sampling/2);
    theta_me=theta_me+W_me*T_Sampling;
    
    //经过计算的角度一定要规整到(-PI, PI]
    if(theta_me > PI)
        theta_me -= 2*PI;
    else if(theta_me < -PI)
        theta_me += 2*PI;

        
        
}


void PathPlanning(void)    //路径规划
{
    static int Flag_Start=0;
    
    double X_des=2.5, Y_des=2.5, Angle_des=0;        //目标位置和姿态
    const double Kd=10,Ka=-7,Kb=0;    
    double ds, da;    
    
    if(Flag_Stop==1) 
    {
        Flag_Start=0;
        Velocity=0,Turn=0;
    }
    else Flag_Start++;
    
    if(Flag_Start<3*1000/10) return;    //等待3s后实验自动开始
    
    //根据目标位置（和姿态），以及当前机器人的位置和姿态，规划出机器人下一时刻的线速度和角速度
    //直接可用的全局变量
    //extern float X_me,Y_me,theta_me;    //小车位置和姿态
    //extern float V_me,W_me;    //小车线速度和角速度-编码器估计值
    //extern float Velocity,Turn;    
    //Velocity为要规划出的线速度,Turn为要规划出的角速度（数学模型顺时针为负 为保持一致故而Ka=-7）（受硬件实现的要求，Turn顺时针为正）

    //补全如下代码    

    
    Velocity=Kd*sqrt(pow(X_des-X_me,2)+pow(Y_des-Y_me,2));

    da=atan2(Y_des-Y_me,X_des-X_me)-theta_me;
        if(da > PI)
        da -= 2*PI;
        else if(da < -PI)
        da += 2*PI;
    Turn=Ka*da;    
}


/**************************************************************************
函数功能：所有的控制代码都在这里面
         5ms定时中断由MPU6050的INT引脚触发
         严格保证采样和数据处理的时间同步                 
**************************************************************************/
void TIM1_UP_IRQHandler(void)  
{    
    if(TIM1->SR&0X0001)//定时中断
    {   

          TIM1->SR&=~(1<<0);                                //===清除定时器中断标志位           
          Flag_Target=!Flag_Target;                                                    //自反，0-1间隔跳变

          if(delay_flag==1)
             {
                 if(++delay_50==10)     delay_50=0,delay_flag=0;   //给主函数提供50ms的精准延时
             }

                 if(Flag_Target==1)                                                  
                {
                     Key();                                       //===扫描按键状态 单击双击可以改变小车运行状态
                     if(Flag_Way==2)
                     {     
                                RD_TSL();                                    //===读取线性CCD数据 
                               Find_CCD_Zhongzhi();                               //===提取中线 
                     }
                     if(Flag_Way==3)        
                     {
                                 Sensor_Left=Get_Adc(3);                //采集左边电感的数据
                                Sensor_Right=Get_Adc(8);               //采集右边电感的数据
                                Sensor_Middle=Get_Adc(2);              //采集中间电感的数据
                            sum=Sensor_Left*1+Sensor_Middle*100+Sensor_Right*199;  //归一化处理
                                Sensor=sum/(Sensor_Left+Sensor_Middle+Sensor_Right);   //求偏差
                     }                         
                }
                if(Flag_Target==0)                                                         //自反，0-1间隔跳变：如下为控制代码，因此控制周期为2*5ms=10ms
             {                                                                                             //为了保证M法测速的时间基准，首先读取编码器数据
                        Encoder_Left=Read_Encoder(2);             //===读取编码器的值                             
                        Encoder_Right=-Read_Encoder(3);                        //===读取编码器的值
                        
                    //任务2-----------------------------
                        //完成如下小车自定位
                        SelfLocalization();                                                //小车自定位
                       //任务2-----------------------------
                    
                    //任务3-----------------------------
                        //完成如下路径规划             
                        PathPlanning();                                                    //路径规划
                        //任务3-----------------------------
                 

                        //任务3时请注释下行 Get_RC();
                        Get_RC();                                                                    //获取遥控命令
                        //-----------------------------------
                 
                        Kinematic_Analysis(Velocity,Turn);             //小车运动学分析   
                        if(Turn_Off(Voltage)==0)                  //===如果不存在异常
                        {
                            //任务1-----------------------------
                            //完成如下两个速度闭环控制
                            Motor_A=Incremental_PI_A(Encoder_Left,Target_A);                   //===速度闭环控制计算电机A最终PWM
                            Motor_B=Incremental_PI_B(Encoder_Right,Target_B);                  //===速度闭环控制计算电机B最终PWM 
                            //任务1-----------------------------
                            
                            Xianfu_Pwm();                                                      //===PWM限幅
                            Set_Pwm(Motor_A,Motor_B,Servo);                                      //===赋值给PWM寄存器  
                        }
                        else
                        Set_Pwm(0,0,SERVO_INIT);                                                          //===赋值给PWM寄存器      
                        Voltage_Temp=Get_battery_volt();                                         //=====读取电池电压        
                        Voltage_Count++;                                                     //=====平均值计数器
                        Voltage_All+=Voltage_Temp;                                           //=====多次采样累积
                        if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====求平均值                                           
             }   
                     if(Flag_Show==0)                Led_Flash(100);
                     else if(Flag_Show==1)    Led_Flash(0);
    }         
} 


/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回  值：无
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int servo)
{
        if(Flag_Way>=2)//巡线模式下，只允许电机正转
            {
              if(motor_a<0)motor_a=0;
                if(motor_b<0)motor_b=0;
            }
        if(motor_a<0)            PWMA1=7200,PWMA2=7200+motor_a;
            else                 PWMA2=7200,PWMA1=7200-motor_a;
        
          if(motor_b<0)            PWMB1=7200,PWMB2=7200+motor_b;
            else                 PWMB2=7200,PWMB1=7200-motor_b;
     //SERVO=servo;    
}

/**************************************************************************
函数功能：限制PWM赋值 
入口参数：无
返回  值：无
**************************************************************************/
void Xianfu_Pwm(void)
{    
      int Amplitude=6900;    //===PWM满幅是7200 限制在6900
    if(Motor_A<-Amplitude) Motor_A=-Amplitude;    
        if(Motor_A>Amplitude)  Motor_A=Amplitude;    
      if(Motor_B<-Amplitude) Motor_B=-Amplitude;    
        if(Motor_B>Amplitude)  Motor_B=Amplitude;        
//        if(Servo<(SERVO_INIT-200))     Servo=SERVO_INIT-200;      //舵机限幅
//        if(Servo>(SERVO_INIT+200))    Servo=SERVO_INIT+200;          //舵机限幅
}
/**************************************************************************
函数功能：按键修改小车运行状态 
入口参数：无
返回  值：无
**************************************************************************/
void Key(void)
{    
    u8 tmp,tmp2;
    tmp=click(); 
    if(tmp==1)Flag_Stop=!Flag_Stop;//单击控制小车的启停
    //if(tmp==2)Flag_Show=!Flag_Show;//双击控制小车的显示状态
    tmp2=Long_Press();          
  if(tmp2==1)Flag_Show=!Flag_Show;//控制小车的显示状态
}

/**************************************************************************
函数功能：异常关闭电机
入口参数：电压
返回  值：1：异常  0：正常
**************************************************************************/
u8 Turn_Off( int voltage)
{
        u8 temp;
            if(voltage<740||Flag_Stop==1)//电池电压低于7.4V关闭电机
            {                                                    
      temp=1;                                            
      }
            else
      temp=0;
      return temp;            
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回  值：unsigned int
**************************************************************************/
int myabs(int a)
{            
      int temp;
        if(a<0)  temp=-a;  
      else temp=a;
      return temp;
}
/**************************************************************************
函数功能：增量PI控制器
入口参数：编码器测量值，目标速度
返回  值：电机PWM
**************************************************************************/
int Incremental_PI_A (int Encoder,int Target)
{     
     static int Bias,Pwm,Last_bias;
     Bias=Target-Encoder;                //计算本次偏差
    
     //计算变量Pwm
     Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;   //增量式PI控制器
     //--------------------
    
     Last_bias=Bias;                       //保存上一次偏差 
     return Pwm;                         //增量输出
}

int Incremental_PI_B (int Encoder,int Target)
{     
     static int Bias,Pwm,Last_bias;
     Bias=Target-Encoder;                //计算偏差
    
     //计算变量Pwm
     Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;   //增量式PI控制器
     //--------------------
    
     Last_bias=Bias;                       //保存上一次偏差 
     return Pwm;                         //增量输出
}
/**************************************************************************
函数功能：遥控
入口参数：无
返回  值：无
**************************************************************************/
void Get_RC(void)
{
    int Yuzhi=2;
    static float Bias,Last_Bias;
  float LY,RX;
    if(Flag_Way==0)//蓝牙控制
    {
        if(Flag_Direction==0)      Velocity=0,Turn=0;   //停止
        else if(Flag_Direction==1) Velocity=Bluetooth_Velocity,Turn=0;  //前进
        else if(Flag_Direction==2) Velocity=Bluetooth_Velocity,Turn=20;  //右前
        else if(Flag_Direction==3) Velocity=0,Turn=20;   //舵机向右
        else if(Flag_Direction==4) Velocity=-Bluetooth_Velocity,Turn=-20;  // 右后
        else if(Flag_Direction==5) Velocity=-Bluetooth_Velocity,Turn=0;    //后退
        else if(Flag_Direction==6) Velocity=-Bluetooth_Velocity,Turn=20;  //左后
        else if(Flag_Direction==7) Velocity=0,Turn=-20;                       //舵机向左
        else if(Flag_Direction==8) Velocity=Bluetooth_Velocity,Turn=-20;  //左前
        else if(Flag_Direction==9) Velocity=Velocity_Cmd,Turn=Turn_Cmd;  //上位机下发的速度命令
    }
    else    if(Flag_Way==1)//PS2控制
    {
      LY=PS2_LY-128;     //计算偏差
        RX=PS2_RX-128;
        if(LY>-Yuzhi&&LY<Yuzhi)LY=0;   //小角度设为死区 防止抖动出现异常
      if(RX>-Yuzhi&&RX<Yuzhi)RX=0;
         Velocity=-LY/2;      //速度和摇杆的力度相关。
         Turn=RX/4;
        
         Velocity/=2;      //降低速度
         Turn/=2;         
    }
        else    if(Flag_Way==2)//CCD巡线
    {
         Velocity=45;       //CCD巡线模式的速度
         Bias=CCD_Zhongzhi-64;   //提取偏差
         Turn=Bias*0.2+(Bias-Last_Bias)*1; //PD控制
         Last_Bias=Bias;   //保存上一次的偏差
    }
        else    if(Flag_Way==3)//电磁巡线
    {
         Velocity=45;      //电磁巡线模式下的速度
         Bias=100-Sensor;  //提取偏差
         Turn=abs(Bias)*Bias*0.008+Bias*0.02+(Bias-Last_Bias)*3; //
         Last_Bias=Bias;   //上一次的偏差
    }
}
/**************************************************************************
函数功能：线性CCD取中值
入口参数：无
返回  值：无
**************************************************************************/
void  Find_CCD_Zhongzhi(void)
{ 
     static u16 i,j,Left,Right,Last_CCD_Zhongzhi;
     static u16 value1_max,value1_min;
    
       value1_max=ADV[0];  //动态阈值算法，读取最大和最小值
     for(i=5;i<123;i++)   //两边各去掉5个点
     {
        if(value1_max<=ADV[i])
        value1_max=ADV[i];
     }
       value1_min=ADV[0];  //最小值
     for(i=5;i<123;i++) 
     {
        if(value1_min>=ADV[i])
        value1_min=ADV[i];
     }
   CCD_Yuzhi=(value1_max+value1_min)/2;      //计算出本次中线提取的阈值
     for(i = 5;i<118; i++)   //寻找左边跳变沿
    {
        if(ADV[i]>CCD_Yuzhi&&ADV[i+1]>CCD_Yuzhi&&ADV[i+2]>CCD_Yuzhi&&ADV[i+3]<CCD_Yuzhi&&ADV[i+4]<CCD_Yuzhi&&ADV[i+5]<CCD_Yuzhi)
        {    
            Left=i;
            break;    
        }
    }
     for(j = 118;j>5; j--)//寻找右边跳变沿
  {
        if(ADV[j]<CCD_Yuzhi&&ADV[j+1]<CCD_Yuzhi&&ADV[j+2]<CCD_Yuzhi&&ADV[j+3]>CCD_Yuzhi&&ADV[j+4]>CCD_Yuzhi&&ADV[j+5]>CCD_Yuzhi)
        {    
          Right=j;
          break;    
        }
  }
    CCD_Zhongzhi=(Right+Left)/2;//计算中线位置
    if(myabs(CCD_Zhongzhi-Last_CCD_Zhongzhi)>90)   //计算中线的偏差，如果太大
    CCD_Zhongzhi=Last_CCD_Zhongzhi;    //则取上一次的值
    Last_CCD_Zhongzhi=CCD_Zhongzhi;  //保存上一次的偏差
}

Answer 1

小车的移动控制逻辑主要集中在TIM1_UP_IRQHandler函数中。为了在小车移动到终点的过程中经过特定点，可以在PathPlanning函数中添加对特定点的判断和处理逻辑。

void PathPlanning(void)
{
    // 在这里添加您的路径规划逻辑
    // 根据小车当前位置(X_me, Y_me)判断是否需要经过特定点，然后调整Velocity和Turn

    // 示例：假设目标点是(3.0, 3.0)，并且通过特定点(2.0, 2.0)
    double targetX = 3.0;
    double targetY = 3.0;
    double specificPointX = 2.0;
    double specificPointY = 2.0;

    // 计算小车当前位置到目标点和特定点的距离
    double distanceToTarget = sqrt(pow(targetX - X_me, 2) + pow(targetY - Y_me, 2));
    double distanceToSpecificPoint = sqrt(pow(specificPointX - X_me, 2) + pow(specificPointY - Y_me, 2));

    // 如果小车距离特定点较近，调整Velocity和Turn
    if (distanceToSpecificPoint < 0.5) {
        Velocity = 20.0; // 适当调整速度
        Turn = 10.0;     // 适当调整转向
    } else {
        // 否则，按照默认路径规划继续移动
        Velocity = 40.0; // 默认速度
        Turn = 0.0;      // 默认转向
    }

    // 其他路径规划逻辑...

    // 在这里添加其他路径规划逻辑
}