闭环思维之follow through和及时反馈

本文探讨了闭环思维在软件开发中的重要性,强调了跟进流程和及时反馈的价值。闭环思维包括彻底跟进和主动反馈,有助于预估业务场景,优化代码扩展性和与产品团队的有效沟通。及时反馈则能加强与领导的沟通,避免项目延期的意外。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

上周晚上一点多大佬果然大佬们都精神好 在企业微信群分享了他的一些思考,看到后觉得太有道理,深受启发,对照着自己平常的工作方式,聊下自己的体会,写出来分享给大家

以下是原话:

····省略一大段 ····

闭环思维主要是两点,第一凡事要 follow through,跟进到底; 第二是及时反馈,主动反馈

  1. 推一个流程,不是流程发布了就完事了,如果没有后续的流程宣讲,监督,反馈,改进,就不是闭环,流程就得不到真正落地
  2. 开发一个功能,不是提测了就完事了,也不是上线就完事了,还有上线后的功能检查,用户反馈,都做到位了,才是闭环
  3. 大家做事情,如果有遇到你的 leader 或者 peer 主动来问你进度,那你就要反思了,反思自己的主动反馈是不是做得不够,说到主动反馈,特别是跟自己leader的主动反馈,其实很多人都做得不够,很多人担心会不会打扰到 leader,leader 会不会觉得烦,等你做了 leader 后,你就会明白,leader 不拍打扰,不会烦,leader 最怕下属给他 surprise,平时不反馈,以为一切正常,突然在 deadline 到来前说出问题了,要延期了,如果遇到重大项目,关键项目,出现这种 surprise,leader 会吐血

····省略一大段 拍手叫好的马屁 ····

看到这段话后,我反思了下我自己的工作方式,挺有感触的,聊聊自己的理解。

闭环思维

平常自己把代码码完,部署好后,扔给测试,自己就去接着码别的代码去了,测试有 bug 再改,也没有了解过写这段代码是为了啥业务,后续推进是怎么样的,用户用的感觉这么样。管它勒,没空~~,我还要去看看 流行的 Flutter,single-spa,又出了哪些新框架,内心觉得自己很勤快啊,没划水~~,一般这种跟进问题其实心里觉得都是产品经理或者项目经理该做的,自己只要把代码码好就可以了, 思考了下如果跟进了这个流程了,在编程的过程,视角会在一个更高的地方,我把这个闭环拆成 2 个维度 1: 以业务为主线,牵涉到 开发,产品,运营这条线; 2: 以项目架构为主线,牵涉到 前端,后端,项目部署,认证体系,鉴权体系,数据库,日志这条线

以业务为主线

第一,最直接的好处是能预估到接下来可能遇到的业务场景,提前在代码层面留好口子,方便扩展。 第二,当熟悉目前做的这件事事干什么,效果怎么样,在接下来和产品开需求评审的时候就可以提让产品信服的建议,产品和开发最常见的问题,就是产品提出某个需求,开发觉得很傻逼,不想做,产品让开发说个 1,2,3理由,开发来又说不出原因,来回就是这个功能没意义,实现有难度..... ,要是很明白刚才说的这些,就能说出个让产品信服的 1,2,3 来

以项目架构为主线

这个意思是 弄明白当前项目的架构,部署,登陆认证,鉴权....,而不是仅仅停留在 后端给 API,负责渲染就可以了,这样的好处是 能明白自己目前做的东西,处在项目架构的哪个位置,出现 API 调不通可能出现在哪个整个架构的哪个地方 还有就是和后端交流 撕逼 也有话语权,本质上前端随着工作年限的增加,自然而然会参与到 项目的架构设计,接口定义中去,而且前后端很多设计思想是相同的(比方发布订阅这套,前后端都有),了解这些都有利于自己在技术视野的成长,也方面和别人吹水。

及时反馈

这个感触更深了,之前在 SGM 负责 围绕 Angular 框架的基建工作,从来没主动给韩老大汇报下自己工作状况,除了每周的周会(有时候也不开)讲讲自己的工作情况,平常在基本没有给他讲自己现在做的事,心里想着 和上面真是一模一样,担心会不会打扰到 Leader 啊 , Leader 这会可能在开会啊,可能在开车啊,可能在聊骚啊,反正就是各种心里想的理由,在开发过程中遇到些问题,能通过代码搞定,绝不会让他找找啥资源,看能调动下不,有时候韩老大就会主动过来问下,现在在做啥,遇到啥问题没有,需不需要外部资源,幸好自己还比较靠谱,没捅啥娄子;

跟 Leader 多沟通,多汇报,讲自己遇到的困难,讲自己的想法,讲自己的解决思路,这样 Leader 心里不慌,遇到问题给你出主意,协调外部资源,比自己一个人埋头吭哧吭哧强多了,领导知道你的想法,这样也能更加了解你做事风格,擅长点,以后也好给自己安排任务。

及时反馈 最重要的一个点,应该是沟通,时时牢记, 主动沟通,和 Leader沟通,和团队同事沟通,和兄弟部门沟通。

以上是我的心得体会,分享给大家,欢迎留言讨论。

#include "balance.h" #include "TrackModule.h" int Time_count=0; //Time variable //计时变量 u8 Lidar_Detect = Lidar_Detect_ON; //电磁巡线模式雷达检测障碍物,默认开启 int RC_Lidar_Avoid_FLAG = RC_Avoid_OFF; //遥控模式雷达避障,默认关闭 u8 Mode; float RC_Velocity_CCD=350,RC_Velocity_ELE=350; float PS2_Velocity,PS2_Turn_Velocity; //遥控控制的速度 Encoder OriginalEncoder; //Encoder raw data //编码器原始数据 u8 Lidar_Success_Receive_flag=0,Lidar_flag_count=0;//雷达在线标志位,雷达掉线计数器 //粗延时函数,微秒 void delay1_us(u16 time) { u16 i=0; while(time--) { i=10; //自己定义 while(i--) ; } } //毫秒级的延时 void delay1_ms(u16 time) { u16 i=0; while(time--) { i=12000; //自己定义 while(i--) ; } } /************************************************************************** Function: The inverse kinematics solution is used to calculate the target speed of each wheel according to the target speed of three axes Input : X and Y, Z axis direction of the target movement speed Output : none 函数功能:运动学逆解,根据三轴目标速度计算各车轮目标转速 入口参数:XY、Z轴方向的目标运动速度 返回 值:无 **************************************************************************/ void Drive_Motor(float Vx,float Vy,float Vz) { float amplitude=3.5; //Wheel target speed limit //车轮目标速度限幅 // if((Mode == PS2_Control_Mode||Mode == APP_Control_Mode)&&RC_Lidar_Avoid_FLAG&&Lidar_Success_Receive_flag)//app以及ps2控制下,判断是否开启遥控避障 // { // RC_Lidar_Aviod(&Vx,&Vy,&Vz); // } // //Speed smoothing is enabled when moving the omnidirectional trolley // //全向移动小车才开启速度平滑处理 // if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) // { // Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //对输入速度进行平滑处理 // // //Get the smoothed data // //获取平滑处理后的数据 // Vx=smooth_control.VX; // Vy=smooth_control.VY; // Vz=smooth_control.VZ; // } // // //Mecanum wheel car // //麦克纳姆轮小车 // if (Car_Mode==Mec_Car) // { // //Inverse kinematics //运动学逆解 // MOTOR_A.Target = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing); // MOTOR_B.Target = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing); // MOTOR_C.Target = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing); // MOTOR_D.Target = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing); // // //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 // MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); // MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); // MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); // MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude); // } // // //Omni car // //全向轮小车 // else if (Car_Mode==Omni_Car) // { // //Inverse kinematics //运动学逆解 // MOTOR_A.Target = Vy + Omni_turn_radiaus*Vz; // MOTOR_B.Target = -X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz; // MOTOR_C.Target = +X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz; // // //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 // MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); // MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); // MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); // MOTOR_D.Target=0; //Out of use //没有使用到 // } // // //Ackermann structure car // //阿克曼小车 // else if (Car_Mode==Akm_Car) // { // //Ackerman car specific related variables //阿克曼小车专用相关变量 // float R, Ratio=636.56, AngleR, Angle_Servo; // // // For Ackerman small car, Vz represents the front wheel steering Angle // //对于阿克曼小车Vz代表右前轮转向角度 // AngleR=Vz; // R=Axle_spacing/tan(AngleR)-0.5f*Wheel_spacing; // // // Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad // //前轮转向角度限幅(舵机控制前轮转向角度),单位:rad // AngleR=target_limit_float(AngleR,-0.49f,0.32f); // // //Inverse kinematics //运动学逆解 // if(AngleR!=0) // { // MOTOR_A.Target = Vx*(R-0.5f*Wheel_spacing)/R; // MOTOR_B.Target = Vx*(R+0.5f*Wheel_spacing)/R; // } // else // { // MOTOR_A.Target = Vx; // MOTOR_B.Target = Vx; // } // // The PWM value of the servo controls the steering Angle of the front wheel // //舵机PWM值,舵机控制前轮转向角度 // //Angle_Servo = -0.628f*pow(AngleR, 3) + 1.269f*pow(AngleR, 2) - 1.772f*AngleR + 1.573f; // Angle_Servo = -0.628f*pow(AngleR, 3) + 1.269f*pow(AngleR, 2) - 1.772f*AngleR + 1.755f; // Servo=SERVO_INIT + (Angle_Servo - 1.755f)*Ratio; // // //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 // MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); // MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); // MOTOR_C.Target=0; //Out of use //没有使用到 // MOTOR_D.Target=0; //Out of use //没有使用到 // Servo=target_limit_int(Servo,800,2200); //Servo PWM value limit //舵机PWM值限幅 // } // // //Differential car // //差速小车 // else if (Car_Mode==Diff_Car) // { //Inverse kinematics //运动学逆解 MOTOR_A.Target = Vx - Vz * Wheel_spacing / 2.0f; //计算出左轮的目标速度 MOTOR_B.Target = Vx + Vz * Wheel_spacing / 2.0f; //计算出右轮的目标速度 //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); MOTOR_C.Target=0; //Out of use //没有使用到 MOTOR_D.Target=0; //Out of use //没有使用到 // } // // //FourWheel car // //四驱车 // else if(Car_Mode==FourWheel_Car) // { // //Inverse kinematics //运动学逆解 // MOTOR_A.Target = Vx - Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出左轮的目标速度 // MOTOR_B.Target = Vx - Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出左轮的目标速度 // MOTOR_C.Target = Vx + Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出右轮的目标速度 // MOTOR_D.Target = Vx + Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出右轮的目标速度 // // //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 // MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); // MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); // MOTOR_C.Target=target_limit_float( MOTOR_C.Target,-amplitude,amplitude); // MOTOR_D.Target=target_limit_float( MOTOR_D.Target,-amplitude,amplitude); // } // // //Tank Car // //履带车 // else if (Car_Mode==Tank_Car) // { // //Inverse kinematics //运动学逆解 // MOTOR_A.Target = Vx - Vz * (Wheel_spacing) / 2.0f; //计算出左轮的目标速度 // MOTOR_B.Target = Vx + Vz * (Wheel_spacing) / 2.0f; //计算出右轮的目标速度 // // //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 // MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); // MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); // MOTOR_C.Target=0; //Out of use //没有使用到 // MOTOR_D.Target=0; //Out of use //没有使用到 // } } /************************************************************************** Function: FreerTOS task, core motion control task Input : none Output : none 函数功能:FreeRTOS任务,核心运动控制任务 入口参数:无 返回 值:无 **************************************************************************/ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { static u8 Count_CCD = 0; //调节CCD控制频率 static u8 last_mode = 0; if(htim->Instance==TIM12) { // //TIM12->SR &= ~(0x01); // //Time count is no longer needed after 30 seconds // //时间计数,30秒后不再需要 // if(Time_count<3000)Time_count++; // //Get the encoder data, that is, the real time wheel speed, // //and convert to transposition international units // //获取编码器数据,即车轮实时速度,并转换位国际单位 // Get_Velocity_Form_Encoder(); //// if(++Lidar_flag_count==10) Lidar_Success_Receive_flag=0,Lidar_flag_count=0,RC_Lidar_Avoid_FLAG=RC_Avoid_OFF; //100ms清零雷达接收到数据标志位 //// switch(User_Key_Scan()) //// { //// case 1: //单击用来切换模式 //// Mode++; //// if(Mode == ELE_Line_Patrol_Mode) //// { //// ELE_ADC_Init(); //初始化电磁巡线模式 //// } ////// //// else if(Mode == CCD_Line_Patrol_Mode) //CCD巡线模式 //// { //// CCD_Init(); //CCD初始化,CCD模块电磁巡线模块共用一个接口,两个不能同时使用 //// } //// else if(Mode > 6) //5种模式循环切换 //// { //// Mode = 0; //// } //// break; //// case 2: //电磁巡线状态时,双击可以打开/关闭雷达检测障碍物,默认打开 //// if(Mode == ELE_Line_Patrol_Mode) //电磁巡线控制模式下 //// { //// Lidar_Detect = !Lidar_Detect; //// if(Lidar_Detect == Lidar_Detect_OFF) //// memset(Dataprocess,0, sizeof(PointDataProcessDef)*225); //用于雷达检测障碍物的数组清零 //// }else if(Mode == APP_Control_Mode||Mode ==PS2_Control_Mode){ //// RC_Lidar_Avoid_FLAG=!RC_Lidar_Avoid_FLAG; //// if(RC_Lidar_Avoid_FLAG == RC_Avoid_OFF) //// memset(Dataprocess,0, sizeof(PointDataProcessDef)*225); //用于雷达检测障碍物的数组清零 //// } //// break; //// } //// if(last_mode != Mode) //每次切换模式需要刷新屏幕 //// { //// last_mode++; //// OLED_Clear(); //// if(last_mode>6) //// last_mode = 0; //// } //// //// if(Mode != ELE_Line_Patrol_Mode) //// Buzzer_Alarm(0); //// if(Mode == APP_Control_Mode) Get_RC(); //Handle the APP remote commands //处理APP遥控命令 //// else if(Mode == PS2_Control_Mode) PS2_Control(); //Handle PS2 controller commands //处理PS2手柄控制命令 //// else if(Mode == Lidar_Avoid_Mode) Lidar_Avoid(); //Avoid Mode //避障模式 //// else if(Mode == Lidar_Follow_Mode) Lidar_Follow(); //Follow Mode //跟随模式 //// else if(Mode == Lidar_Along_Mode) Lidar_along_wall(); //Along Mode //走直线模式 //// else if(Mode == ELE_Line_Patrol_Mode) //// { //// //// //// Get_RC_ELE(); //ELE模式 ////// delay1_ms(2); //// } //// else //CCD模式 //// { //// if(++Count_CCD == 2) //调节控制频率,4*5 = 20ms控制一次 //// { //// Count_CCD = 0; //// //// Get_RC_CCD(); //// } //// else if(Count_CCD>2) //// Count_CCD = 0; //// } // // // IRDM_line_inspection_V2(); // Drive_Motor(Move_X,Move_Y,Move_Z); // // // //If there is no abnormity in the battery voltage, and the enable switch is in the ON position, // //and the software failure flag is 0 // //如果电池电压不存在异常,而且使能开关在ON档位,而且软件失能标志位为0 // if(Turn_Off(Voltage)==0) // { // //Speed closed-loop control to calculate the PWM value of each motor, // //PWM represents the actual wheel speed // //速度闭环控制计算各电机PWM值,PWM代表车轮实际转速 // MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target); // MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target); // MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target); // MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target); // Limit_Pwm(16500) ; // //Set different PWM control polarity according to different car models // //根据不同小车型号设置不同的PWM控制极性 // switch(Car_Mode) // { // case Mec_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Mecanum wheel car //麦克纳姆轮小车 // case Omni_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Omni car //全向轮小车 // case Akm_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, Servo); break; //Ackermann structure car //阿克曼小车 // case Diff_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Differential car //两轮差速小车 // case FourWheel_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //FourWheel car //四驱车 // case Tank_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Tank Car //履带车 // } // } // //If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0 // //如果Turn_Off(Voltage)返回值为1,不允许控制小车进行运动,PWM值设置为0 // else Set_Pwm(0,0,0,0,0); //----------------云台移植测试-------------------// Xianfu_Pwm(); Velocity1=Position_PID_1(Position1,Target1); //舵机PID控制,可以根据目标位置进行速度调整,离目标位置越远速度越快 Velocity2=Position_PID_2(Position2,Target2); //舵机PID控制,可以根据目标位置进行速度调整,离目标位置越远速度越快 Xianfu_Velocity(); Set_Pwm_PTZ(Velocity1,Velocity2); //赋值给PWM寄存器 } } /************************************************************************** Function: Assign a value to the PWM register to control wheel speed and direction Input : PWM Output : none 函数功能:赋值给PWM寄存器,控制车轮转速与方向 入口参数:PWM 返回 值:无 **************************************************************************/ void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo) { if(motor_a<0) PWMA2=16800,PWMA1=16800+motor_a; else PWMA1=16800,PWMA2=16800-motor_a; if(motor_b<0) PWMB1=16800,PWMB2=16800+motor_b; else PWMB2=16800,PWMB1=16800-motor_b; if(motor_c<0) PWMC1=16800,PWMC2=16800+motor_c; else PWMC2=16800,PWMC1=16800-motor_c; if(motor_d<0) PWMD2=16800,PWMD1=16800+motor_d; else PWMD1=16800,PWMD2=16800-motor_d; //Servo control //舵机控制 Servo_PWM = servo; } /************************************************************************** Function: Limit PWM value Input : Value Output : none 函数功能:限制PWM值 入口参数:幅值 返回 值:无 **************************************************************************/ void Limit_Pwm(int amplitude) { MOTOR_A.Motor_Pwm=target_limit_float(MOTOR_A.Motor_Pwm,-amplitude,amplitude); MOTOR_B.Motor_Pwm=target_limit_float(MOTOR_B.Motor_Pwm,-amplitude,amplitude); MOTOR_C.Motor_Pwm=target_limit_float(MOTOR_C.Motor_Pwm,-amplitude,amplitude); MOTOR_D.Motor_Pwm=target_limit_float(MOTOR_D.Motor_Pwm,-amplitude,amplitude); } /************************************************************************** Function: Limiting function Input : Value Output : none 函数功能:限幅函数 入口参数:幅值 返回 值:无 **************************************************************************/ float target_limit_float(float insert,float low,float high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } int target_limit_int(int insert,int low,int high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } /************************************************************************** Function: Check the battery voltage, enable switch status, software failure flag status Input : Voltage Output : Whether control is allowed, 1: not allowed, 0 allowed 函数功能:检查电池电压、使能开关状态、软件失能标志位状态 入口参数:电压 返回 值:是否允许控制,1:不允许,0允许 **************************************************************************/ u8 Turn_Off( int voltage) { u8 temp; if(voltage<10||EN==0||Flag_Stop==1) { temp=1; } else temp=0; return temp; } /************************************************************************** Function: Calculate absolute value Input : long int Output : unsigned int 函数功能:求绝对值 入口参数:long int 返回 值:unsigned int **************************************************************************/ u32 myabs(long int a) { u32 temp; if(a<0) temp=-a; else temp=a; return temp; } /************************************************************************** Function: Incremental PI controller Input : Encoder measured value (actual speed), target speed Output : Motor PWM According to the incremental discrete PID formula pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)] e(k) represents the current deviation e(k-1) is the last deviation and so on PWM stands for incremental output In our speed control closed loop system, only PI control is used pwm+=Kp[e(k)-e(k-1)]+Ki*e(k) 函数功能:增量式PI控制器 入口参数:编码器测量值(实际速度),目标速度 返回 值:电机PWM 根据增量式离散PID公式 pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)] e(k)代表本次偏差 e(k-1)代表上一次的偏差 以此类推 pwm代表增量输出 在我们的速度控制闭环系统里面,只使用PI控制 pwm+=Kp[e(k)-e(k-1)]+Ki*e(k) **************************************************************************/ int Incremental_PI_A (float Encoder,float Target) { static float Bias,Pwm,Last_bias; Bias=Target-Encoder; //Calculate the deviation //计算偏差 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; if(Pwm>16800)Pwm=16800; if(Pwm<-16800)Pwm=-16800; Last_bias=Bias; //Save the last deviation //保存上一次偏差 return Pwm; } int Incremental_PI_B (float Encoder,float Target) { static float Bias,Pwm,Last_bias; Bias=Target-Encoder; //Calculate the deviation //计算偏差 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; if(Pwm>16800)Pwm=16800; if(Pwm<-16800)Pwm=-16800; Last_bias=Bias; //Save the last deviation //保存上一次偏差 return Pwm; } int Incremental_PI_C (float Encoder,float Target) { static float Bias,Pwm,Last_bias; Bias=Target-Encoder; //Calculate the deviation //计算偏差 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; if(Pwm>16800)Pwm=16800; if(Pwm<-16800)Pwm=-16800; Last_bias=Bias; //Save the last deviation //保存上一次偏差 return Pwm; } int Incremental_PI_D (float Encoder,float Target) { static float Bias,Pwm,Last_bias; Bias=Target-Encoder; //Calculate the deviation //计算偏差 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; if(Pwm>16800)Pwm=16800; if(Pwm<-16800)Pwm=-16800; Last_bias=Bias; //Save the last deviation //保存上一次偏差 return Pwm; } /************************************************************************** Function: Processes the command sent by APP through usart 2 Input : none Output : none 函数功能:对APP通过串口2发送过来的命令进行处理 入口参数:无 返回 值:无 **************************************************************************/ void Get_RC(void) { u8 Flag_Move=1; if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //全向轮运动小车可以进行横向移动 { switch(Flag_Direction) //Handle direction control commands //处理方向控制命令 { case 1: Move_X=RC_Velocity; Move_Y=0; Flag_Move=1; break; case 2: Move_X=RC_Velocity; Move_Y=-RC_Velocity; Flag_Move=1; break; case 3: Move_X=0; Move_Y=-RC_Velocity; Flag_Move=1; break; case 4: Move_X=-RC_Velocity; Move_Y=-RC_Velocity; Flag_Move=1; break; case 5: Move_X=-RC_Velocity; Move_Y=0; Flag_Move=1; break; case 6: Move_X=-RC_Velocity; Move_Y=RC_Velocity; Flag_Move=1; break; case 7: Move_X=0; Move_Y=RC_Velocity; Flag_Move=1; break; case 8: Move_X=RC_Velocity; Move_Y=RC_Velocity; Flag_Move=1; break; default: Move_X=0; Move_Y=0; Flag_Move=0; break; } if(Flag_Move==0) { //If no direction control instruction is available, check the steering control status //如果无方向控制指令,检查转向控制状态 if (Flag_Left ==1) Move_Z= PI/2*(RC_Velocity/500); //left rotation //左自转 else if(Flag_Right==1) Move_Z=-PI/2*(RC_Velocity/500); //right rotation //右自转 else Move_Z=0; //stop //停止 } } else //Non-omnidirectional moving trolley //非全向移动小车 { switch(Flag_Direction) //Handle direction control commands //处理方向控制命令 { case 1: Move_X=+RC_Velocity; Move_Z=0; break; case 2: Move_X=+RC_Velocity; Move_Z=-PI/2; break; case 3: Move_X=0; Move_Z=-PI/2; break; case 4: Move_X=-RC_Velocity; Move_Z=-PI/2; break; case 5: Move_X=-RC_Velocity; Move_Z=0; break; case 6: Move_X=-RC_Velocity; Move_Z=+PI/2; break; case 7: Move_X=0; Move_Z=+PI/2; break; case 8: Move_X=+RC_Velocity; Move_Z=+PI/2; break; default: Move_X=0; Move_Z=0; break; } if (Flag_Left ==1) Move_Z= PI/2; //left rotation //左自转 else if(Flag_Right==1) Move_Z=-PI/2; //right rotation //右自转 } //Z-axis data conversion //Z轴数据转化 if(Car_Mode==Akm_Car) { //Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed //阿克曼结构小车转换为前轮转向角度 Move_Z=Move_Z*2/9; } else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car) { if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理 Move_Z=Move_Z*RC_Velocity/500; } //Unit conversion, mm/s -> m/s //单位转换,mm/s -> m/s Move_X=Move_X/1000; Move_Y=Move_Y/1000; Move_Z=Move_Z; //Control target value is obtained and kinematics analysis is performed //得到控制目标值,进行运动学分析 Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** Function: Read the encoder value and calculate the wheel speed, unit m/s Input : none Output : none 函数功能:读取编码器数值并计算车轮速度,单位m/s 入口参数:无 返回 值:无 **************************************************************************/ void Get_Velocity_Form_Encoder(void) { //Retrieves the original data of the encoder //获取编码器的原始数据 float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; OriginalEncoder.A=Read_Encoder(2); OriginalEncoder.B=Read_Encoder(3); OriginalEncoder.C=Read_Encoder(4); OriginalEncoder.D=Read_Encoder(5); //Decide the encoder numerical polarity according to different car models //根据不同小车型号决定编码器数值极性 switch(Car_Mode) { case Mec_Car: Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr=OriginalEncoder.B; Encoder_C_pr= -OriginalEncoder.C; Encoder_D_pr= -OriginalEncoder.D; break; case Omni_Car: Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= -OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break; case Akm_Car: Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break; case Diff_Car: Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break; case FourWheel_Car: Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr=OriginalEncoder.B; Encoder_C_pr= -OriginalEncoder.C; Encoder_D_pr= -OriginalEncoder.D; break; case Tank_Car: Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr= -OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break; } //The encoder converts the raw data to wheel speed in m/s //编码器原始数据转换为车轮速度,单位m/s MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; } /************************************************************************** Function: Smoothing the three axis target velocity Input : Three-axis target velocity Output : none 函数功能:对三轴目标速度做平滑处理 入口参数:三轴目标速度 返回 值:无 **************************************************************************/ void Smooth_control(float vx,float vy,float vz) { float step=0.05; if (vx>0) smooth_control.VX+=step; else if(vx<0) smooth_control.VX-=step; else if(vx==0) smooth_control.VX=smooth_control.VX*0.9f; if (vy>0) smooth_control.VY+=step; else if(vy<0) smooth_control.VY-=step; else if(vy==0) smooth_control.VY=smooth_control.VY*0.9f; if (vz>0) smooth_control.VZ+=step; else if(vz<0) smooth_control.VZ-=step; else if(vz==0) smooth_control.VZ=smooth_control.VZ*0.9f; smooth_control.VX=target_limit_float(smooth_control.VX,-float_abs(vx),float_abs(vx)); smooth_control.VY=target_limit_float(smooth_control.VY,-float_abs(vy),float_abs(vy)); smooth_control.VZ=target_limit_float(smooth_control.VZ,-float_abs(vz),float_abs(vz)); } /************************************************************************** Function: Floating-point data calculates the absolute value Input : float Output : The absolute value of the input number 函数功能:浮点型数据计算绝对值 入口参数:浮点数 返回 值:输入数的绝对值 **************************************************************************/ float float_abs(float insert) { if(insert>=0) return insert; else return -insert; } /************************************************************************** Function: PS2_Control Input : none Output : none 函数功能:PS2手柄控制 入口参数: 无 返回 值:无 **************************************************************************/ void PS2_Control(void) { int LY,RX,LX; //手柄ADC的值 int Threshold=20; //阈值,忽略摇杆小幅度动作 static float Key1_Count = 0,Key2_Count = 0; //用于控制读取摇杆的速度 //转化为128到-128的数值 LY=-(PS2_LY-128);//左边Y轴控制前进后退 RX=-(PS2_RX-128);//右边X轴控制转向 LX=-(PS2_LX-128);//左边X轴控制转向,麦轮全向小车专用 if(LY>-Threshold&&LY<Threshold) LY=0; if(RX>-Threshold&&RX<Threshold) RX=0; //忽略摇杆小幅度动作 if(LX>-Threshold&&LX<Threshold) LX=0; if(Strat) //按下start键才可以控制小车 { if (PS2_KEY == PSB_L1) //按下左1键加速(按键在顶上) { if((++Key1_Count) == 20) //调节按键反应速度 { PS2_KEY = 0; Key1_Count = 0; if((PS2_Velocity += X_Step)>MAX_RC_Velocity) //前进最大速度1230 PS2_Velocity = MAX_RC_Velocity; if(Car_Mode != Akm_Car) //非阿克曼车可调节转向速度 { if((PS2_Turn_Velocity += Z_Step)>MAX_RC_Turn_Bias) //转向最大速度325 PS2_Turn_Velocity = MAX_RC_Turn_Bias; } } } else if(PS2_KEY == PSB_R1) //按下右1键减速 { if((++Key2_Count) == 15) { PS2_KEY = 0; Key2_Count = 0; if((PS2_Velocity -= X_Step)<MINI_RC_Velocity) //前后最小速度110 PS2_Velocity = MINI_RC_Velocity; if(Car_Mode != Akm_Car) //非阿克曼车可调节转向速度 { if((PS2_Turn_Velocity -= Z_Step)<MINI_RC_Turn_Velocity)//转向最小速度45 PS2_Turn_Velocity = MINI_RC_Turn_Velocity; } } } else Key2_Count = 0,Key2_Count = 0; //读取到其他按键重新计数 Move_X = (PS2_Velocity/128)*LY; //速度控制,力度表示速度大小 if(Car_Mode == Mec_Car || Car_Mode == Omni_Car) { Move_Y = LX*PS2_Velocity/128; } else { Move_Y = 0; } if(Car_Mode == Akm_Car) //阿克曼车转向控制,力度表示转向角度 Move_Z = RX*(PI/2)/128*2/9; else //其他车型转向控制 { //if(Move_X>=0) Move_Z = (PS2_Turn_Velocity/128)*RX; //转向控制,力度表示转向速度 // else // Move_Z = -(PS2_Turn_Velocity/128)*RX; } } else { Move_X = 0; Move_Y = 0; Move_Z = 0; } Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** 函数功能:CCD巡线,采集3个电感的数据并提取中线 入口参数:无 返回 值:无 **************************************************************************/ void Get_RC_CCD(void) { static float Bias,Last_Bias; float move_z=0; Move_X=RC_Velocity_CCD; //CCD巡线模式线速度 Bias=CCD_Median-64; //提取偏差,64为巡线的中心点 if(Car_Mode == Omni_Car) move_z=-Bias*Omni_Car_CCD_KP*0.1f-(Bias-Last_Bias)*Omni_Car_CCD_KI*0.1f; //PD控制,原理就是使得小车保持靠近巡线的中心点 else if(Car_Mode == Tank_Car) move_z=-Bias*Tank_Car_CCD_KP*0.1f-(Bias-Last_Bias)*Tank_Car_CCD_KI*0.1f; else move_z=-Bias*CCD_KP*0.1f-(Bias-Last_Bias)*CCD_KI*0.1f; Last_Bias=Bias; //保存上一次的偏差 if(Car_Mode==Mec_Car) { Move_Z=move_z*RC_Velocity_CCD/50000; //差速控制原理需要经过此处处理 } else if(Car_Mode==Omni_Car) { Move_Z=move_z*RC_Velocity_CCD/21000; //差速控制原理需要经过此处处理 } else if(Car_Mode==Akm_Car) { Move_Z=move_z/450; //差速控制原理需要经过此处处理 } else if(Car_Mode==Diff_Car) { if(Move_X<0) move_z=-move_z; Move_Z=move_z*RC_Velocity_CCD/67000; //差速控制原理需要经过此处处理 } else if(Car_Mode==Tank_Car) { if(Move_X<0) move_z=-move_z; Move_Z=move_z*RC_Velocity_CCD/50000; //差速控制原理需要经过此处处理 } else if(Car_Mode==FourWheel_Car) { if(Move_X<0) move_z=-move_z; Move_Z=move_z*RC_Velocity_CCD/20100; //差速控制原理需要经过此处处理 } //Z-axis data conversion //Z轴数据转化 //Unit conversion, mm/s -> m/s //单位转换,mm/s -> m/s Move_X=Move_X/1000; Move_Z=Move_Z; Move_Y=0; //Control target value is obtained and kinematics analysis is performed //得到控制目标值,进行运动学分析 Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** 函数功能:电磁巡线,采集3个电感的数据并提取中线 入口参数:无 返回 值:无 **************************************************************************/ void Get_RC_ELE(void) { static float Bias,Last_Bias; float move_z=0; if(Detect_Barrier() == No_Barrier) { Move_X=RC_Velocity_ELE; //电磁巡线模式的速度 Bias=100-Sensor; //提取偏差 if(Car_Mode == Omni_Car) move_z=-Bias* Omni_Car_ELE_KP*0.08f-(Bias-Last_Bias)* Omni_Car_ELE_KI*0.05f; else if(Car_Mode == Tank_Car) move_z=-Bias*Tank_Car_ELE_KP*0.1f-(Bias-Last_Bias)*Tank_Car_ELE_KI*0.1f; else move_z=-Bias* ELE_KP*0.08f-(Bias-Last_Bias)* ELE_KI*0.05f; Last_Bias=Bias; Buzzer_Alarm(0); if(Car_Mode==Mec_Car) { Move_Z=move_z*RC_Velocity_ELE/50000; //差速控制原理需要经过此处处理 } else if(Car_Mode==Omni_Car) { Move_Z=move_z*RC_Velocity_ELE/10800; //差速控制原理需要经过此处处理 } else if(Car_Mode==Diff_Car) { if(Move_X<0) move_z=-move_z; Move_Z=move_z*RC_Velocity_ELE/45000; //差速控制原理需要经过此处处理 } else if(Car_Mode==Tank_Car) { if(Move_X<0) move_z=-move_z; Move_Z=move_z*RC_Velocity_ELE/28000; //差速控制原理需要经过此处处理 } else if(Car_Mode==FourWheel_Car) { if(Move_X<0) move_z=-move_z; Move_Z=move_z*RC_Velocity_ELE/20100; //差速控制原理需要经过此处处理 } else if(Car_Mode==Akm_Car) { Move_Z=move_z/450; //差速控制原理需要经过此处处理 } } else //有障碍物 { Buzzer_Alarm(100); //当电机使能的时候,有障碍物则蜂鸣器报警 Move_X = 0; Move_Z = 0; Move_Y = 0; } //Z-axis data conversion //Z轴数据转化 //Unit conversion, mm/s -> m/s //单位转换,mm/s -> m/s Move_X=Move_X/1000; Move_Z=Move_Z; Move_Y=0; //Control target value is obtained and kinematics analysis is performed //得到控制目标值,进行运动学分析 Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** 函数功能:检测前方是否有障碍物 入口参数:无 返回 值:无 **************************************************************************/ u8 Detect_Barrier(void) { u16 i; u16 point_count = 0; if(Lidar_Detect == Lidar_Detect_ON) { for(i=0;i<1152;i++) //检测是否有障碍物 { if((Dataprocess[i].angle>300)||(Dataprocess[i].angle<60)) { if(0<Dataprocess[i].distance&&Dataprocess[i].distance<700)//700mm内是否有障碍物 point_count++; } } if(point_count > 0)//有障碍物 return Barrier_Detected; else return No_Barrier; } else return No_Barrier; } /************************************************************************** 函数功能:小车避障模式 入口参数:无 返回 值:无 **************************************************************************/ void Lidar_Avoid(void) { u16 i = 0; u8 calculation_angle_cnt = 0; //用于判断225个点中需要做避障的点 float angle_sum = 0; //粗略计算障碍物位于左或者右 u8 distance_count = 0; //距离小于某值的计数 for(i=0;i<1152;i++) //遍历120度范围内的距离数据,共120个点左右的数据 { if((Dataprocess[i].angle>300)||(Dataprocess[i].angle<60)) //避障角度在300-60之间 { if((0<Dataprocess[i].distance)&&(Dataprocess[i].distance<Avoid_Distance)) //距离小于450mm需要避障,只需要120度范围内点 { calculation_angle_cnt++; //计算距离小于避障距离的点个数 if(Dataprocess[i].angle<60) angle_sum += Dataprocess[i].angle; else if(Dataprocess[i].angle>300) angle_sum += (Dataprocess[i].angle-360); //300度到60度转化为-60度到60度 if(Dataprocess[i].distance<Avoid_Min_Distance) //记录小于200mm的点的计数 distance_count++; } } } Move_X = forward_velocity; if(calculation_angle_cnt == 0)//不需要避障 { Move_Z = 0; } else //当距离小于200mm,小车往后退 { if(distance_count>8) { Move_X = -forward_velocity; Move_Z = 0; } else { Move_X = 0; if(angle_sum > 0)//障碍物偏右 { if(Car_Mode == Mec_Car) //麦轮转弯需要把前进速度降低 Move_X = 0; else //其他车型保持原有车速 Move_X = forward_velocity; if(Car_Mode == Akm_Car) Move_Z = PI/4; else if(Car_Mode == Omni_Car) Move_Z=corner_velocity; else Move_Z=other_corner_velocity;//左转 } else //偏左 { if(Car_Mode == Mec_Car) Move_X = 0; else Move_X = forward_velocity; if(Car_Mode == Akm_Car) Move_Z = -PI/4; else if(Car_Mode == Omni_Car) Move_Z=-corner_velocity;//右转 else Move_Z=-other_corner_velocity; } } } Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** 函数功能:小车跟随模式 入口参数:无 返回 值:无 **************************************************************************/ void Lidar_Follow(void) { static u16 cnt = 0; int i; int calculation_angle_cnt = 0; static float angle = 0; //避障的角度 static float last_angle = 0; // u16 mini_distance = 65535; static u8 data_count = 0; //用于滤除一写噪点的计数变量 //需要找出跟随的那个点的角度 for(i = 0; i < 1152; i++) { if((0<Dataprocess[i].distance)&&(Dataprocess[i].distance<Follow_Distance)) { calculation_angle_cnt++; if(Dataprocess[i].distance<mini_distance) { mini_distance = Dataprocess[i].distance; angle = Dataprocess[i].angle; } } } if(angle > 180) //0--360度转换成0--180;-180--0(顺时针) angle -= 360; if((angle-last_angle > 10)||(angle-last_angle < -10)) //做一定消抖,波动大于10度的需要做判断 { if(++data_count > 30) //连续30次采集到的值(300ms后)上次的比大于10度,此时才是认为是有效值 { data_count = 0; last_angle = angle; } } else //波动小于10度的可以直接认为是有效值 { if(++data_count > 10) //连续10次采集到的值(100ms后),此时才是认为是有效值 { data_count = 0; last_angle = angle; } } if(calculation_angle_cnt < 8) //跟随距离小于8且当cnt>40的时候,认为在1600内没有跟随目标 { if(cnt < 40) cnt++; if(cnt >= 40) { Move_X = 0; Move_Z = 0; } } else { cnt = 0; if(Move_X > 0.06f || Move_X < -0.06f) //当Move_X有速度时,转向PID开始调整 { if(mini_distance < 700 && (last_angle > 60 || last_angle < -60)) { Move_Z = -0.0098f*last_angle; //当距离偏小且角度差距过大直接快速转向 } else { Move_Z = -Follow_Turn_PID(last_angle,0); //转向PID,车头永远对着跟随物品 } } else { Move_Z = 0; } if(angle>150 || angle<-150) //如果小车在后方60°需要反方向运动以及快速转弯 { Move_X = -Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); Move_Z = -0.0098f*last_angle; } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); //保持距离保持在500mm } Move_X = target_limit_float(Move_X,-amplitude_limiting,amplitude_limiting); //对前进速度限幅 } Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** 函数功能:小车走直线模式 入口参数:无 返回 值:无 **************************************************************************/ void Lidar_along_wall(void) { static int determine; static u32 target_distance=0; u32 distance; u8 data_count = 0; //用于滤除一写噪点的计数变量 Move_X = forward_velocity; //初始速度 for(int j=0;j<1152;j++) //225 { if(Dataprocess[j].angle>268 && Dataprocess[j].angle<272) //取雷达的4度的点 { if(determine<Limit_time) { target_distance=Dataprocess[j].distance; //雷达捕获第一个距离 determine++; if(determine==(Limit_time-1)) determine=Limit_time; } if(Dataprocess[j].distance<(target_distance+limit_distance))//限制一下雷达的探测距离 { data_count++; distance=Dataprocess[j].distance;//实时距离 } } } if(Car_Mode == Mec_Car || Car_Mode == Omni_Car) //只有麦轮全向可以用Move_Y { Move_Y= Along_Adjust_PID(distance,target_distance); Move_X = forward_velocity; Move_Z = 0; } else //其他车型使用Move_Z保持走直线状态 { Move_Z=Along_Adjust_PID(distance,target_distance); Move_X = forward_velocity; Move_Y = 0; } if(data_count == 0) //当data_count等于0,只有前进速度 { Move_Y = 0; Move_Z = 0; } Drive_Motor(Move_X,Move_Y,Move_Z); } /************************************************************************** 函数功能:小车遥控避障功能 入口参数:无 返回 值:无 **************************************************************************/ void RC_Lidar_Aviod(float *Vx,float *Vy,float *Vz){ int i; int avoid_distance=450;//避障距离 u16 avoid_point_count=0;//范围内需要壁障的点数 int left_angle_sum=0, right_angle_sum=0; int mini_distance_count=0;//最小转弯距离的点数 for(i=0;i<1152;i++) { if(Dataprocess[i].angle>300||Dataprocess[i].angle<60) {//车前避障角度 300°-60° if(0<Dataprocess[i].distance&&Dataprocess[i].distance<avoid_distance) { avoid_point_count++;//需要避障的点数 if(Dataprocess[i].angle<60) { right_angle_sum+=Dataprocess[i].angle; } else if(Dataprocess[i].angle>300) { left_angle_sum+=(Dataprocess[i].angle-360);//300°到359°转化为-60°到-1° } if(Dataprocess[i].distance<Avoid_Min_Distance) {//小于最小避障距离 mini_distance_count++; } } } } if(*Vx>0&&avoid_point_count>8){//需要避障 if(mini_distance_count>=8)//距离小于最小转弯距离要求 { *Vx=*Vx*-0.75f;//后退 *Vz=0; } else { if(right_angle_sum+left_angle_sum>0) {//往左避让 *Vx=*Vx*0.5f;//避障的X轴速度是前进速度的一半 if(Car_Mode==Akm_Car){ *Vz=(PI/2)*4/9; }else{ *Vz=1; } }else{//往右避让 *Vx=*Vx*0.5f;//避障的X轴速度是前进速度的一半 if(Car_Mode==Akm_Car){ *Vz=(-PI/2)*4/9; }else{ *Vz=-1; } } } } } /************************************************************************** 函数功能:限制PWM赋值 入口参数:无 返回 值:无 **************************************************************************/ void Xianfu_Pwm(void) { int Amplitude_H1=1200, Amplitude_L1=300; //舵机脉宽极限值,即限制舵机转角13.5°-256.5°中心位置为750,运动范围为±450,(750-250)/90=5.5,5.5*80=450 int Amplitude_H2=1200, Amplitude_L2=300; //舵机脉宽极限值,即限制舵机转角9°-171°,中心位置为750,运动范围为±450,(750-250)/90=5.5,5.5*80=450 if(Target1<Amplitude_L1) Target1=Amplitude_L1; if(Target1>Amplitude_H1) Target1=Amplitude_H1; if(Target2<Amplitude_L2) Target2=Amplitude_L2; if(Target2>Amplitude_H2) Target2=Amplitude_H2; } /************************************************************************* 函数功能:位置式PID控制器 入口参数:编码器测量位置信息,目标位置 返回 值:电机PWM增量 **************************************************************************/ float Position_PID_1(float Position,float Target) { //增量输出 static float Bias,Pwm,Integral_bias,Last_Bias; Bias=Target-Position; //计算偏差 Integral_bias+=Bias; //求出偏差的积分 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100; //位置式PID控制器 Last_Bias=Bias; //保存上一次偏差 return Pwm; } /************************************************************************* 函数功能:位置式PID控制器 入口参数:编码器测量位置信息,目标位置 返回 值:电机PWM增量 **************************************************************************/ float Position_PID_2(float Position,float Target) { //增量输出 static float Bias,Pwm,Integral_bias,Last_Bias; Bias=Target-Position; //计算偏差 Integral_bias+=Bias; //求出偏差的积分 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100; //位置式PID控制器 Last_Bias=Bias; //保存上一次偏差 return Pwm; } /************************************************************************** 函数功能:限制速度 入口参数:无 返回 值:无 **************************************************************************/ void Xianfu_Velocity(void) { int Amplitude_H=Speed, Amplitude_L=-Speed; if(Velocity1<Amplitude_L) Velocity1=Amplitude_L; if(Velocity1>Amplitude_H) Velocity1=Amplitude_H; if(Velocity2<Amplitude_L) Velocity2=Amplitude_L; if(Velocity2>Amplitude_H) Velocity2=Amplitude_H; } /************************************************************************** 函数功能:赋值给PWM寄存器,并且判断转向 入口参数:左轮PWM、右轮PWM 返回 值:无 **************************************************************************/ void Set_Pwm_PTZ(float velocity1,float velocity2) { Position1+=velocity1; //速度的积分,得到舵机的位置 Position2+=velocity2; //赋值给STM32的寄存器 // TIM12->CCR1=Position1; //GPIOA8,云台舵机 // TIM12->CCR2=Position2; //GPIOA11,外臂舵机 TIM8->CCR1=Position1; //GPIOC6,云台舵机 TIM8->CCR2=Position2; //GPIOC7,外臂舵机 }看不懂啊,逐行解释
最新发布
07-30
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值