目录
1.使用带有PID计算和遥控器的工程(最好同时使用了FreeRTOS多线程任务),在原有代码的基础修改。
一:安装底盘
安装X型不易在横向移动的时候产生打滑而造成“漂移”!
二:完成小车全自动画矩形
1.使用带有PID计算和遥控器的工程(最好同时使用了FreeRTOS多线程任务),在原有代码的基础修改。
2.写保护!
在给小车编写代码时第一步最好是先把写保护的代码写了,防止小车不受控制。
可以自己设定一个”停车/刹车“的开关。
对应remote_control.c文件中,找到:
/** * @brief remote control protocol resolution * @param[in] sbus_buf: raw data point * @param[out] rc_ctrl: remote control data struct point * @retval none */ /** * @brief 遥控器协议解析 * @param[in] sbus_buf: 原生数据指针 * @param[out] rc_ctrl: 遥控器数据值 * @retval none */ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl) { if (sbus_buf == NULL || rc_ctrl == NULL) { return; } rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0 rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1 rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2 (sbus_buf[4] << 10)) &0x07ff; rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3 rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right rc_ctrl->mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis rc_ctrl->mouse.y = sbus_buf[8] | (sbus_buf[9] << 8); //!< Mouse Y axis rc_ctrl->mouse.z = sbus_buf[10] | (sbus_buf[11] << 8); //!< Mouse Z axis rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ? rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ? rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET; }
可以看到rc.ch数组的4个元素0,1,2,3分别对应遥控器额4个通道(可查手册)——
左y轴对应rc.ch[3], 右y轴对应rc.ch[1],左x轴对应rc.ch[2],右x轴对应rc.ch[0] 。
而rc.s数组的2个元素0和1则分别对应左右上角的开关——
rc.s[0]=0x0003对应右中 ,rc[0]=0x0001对应右上,rc[0]=0x0002对应右下。
并且注意引用rc.ch和rc.cs的时候这是结构的两个成员,
所以只要通过判断rc.s的值就可以知道开关的状态了。
于是可以得到如下写保护的代码:
if(rc_ctrl.rc.s[0]==0x0001) { //红灯亮表示停车 __HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_3,1000); __HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_1,0); __HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_2,0); for(int i=0;i<4;i++) { give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); //断流,写保护 } can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); osDelay(5); }
通过判断开关状态进入if语句内,在其中将4个电机的转速都置0并调用PID_calc函数进行时变电流的赋值,确保小车可以及时停下。
PS:1.可以顺便点个灯来表示当前小车的状态,像上面的代码就是点红灯表示停车/刹车,如果其他状态是点其他灯的话要注意灭掉其他灯,不然最后就是白灯了。
2.为了确保多线程的每个线程(任务)中有一定时间的延时(比如5ms),确保进程不会卡死,而开关状态却可能不会在短时间内发生改变,所以最好每个if里面都要有Delay,以确保进程不会卡死。
3.还要注意其他判断条件不要用else,最好都要有相应的遥控器开关的判断,这样可以让程序更好的以想要的方式运行。
3.画矩形
else if(rc_ctrl.rc.s[0]==0x0002) { //Â̵ÆÁÁ±íʾÕý³£×ªËÙ __HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_2,1000); __HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_1,0); __HAL_TIM_SET_COMPARE(&htim5,TIM_CHANNEL_3,0); //µçµ÷1~4¶ÔÓ¦ÂÖ×Ó1~4,×óÉϽÇ1ÂÖ ,ÓÒÉϽÇ2ÂÖ £¬ÓÒϽÇ3ÂÖ £¬ ¼ÇµÃµçµ÷ÒªÒ»Ò»¶ÔÓ¦!!!! //1£¬4¸øÕýÖµÍùÇ°£¬2£¬3¸ø¸ºÖµÍùÇ°£¡£¡£¡ //0x200¸ù¾Ýµç»úid¶øÉ裬Èç¹ûµç»úidΪ1~4£¬µÚ¶þ¸ö²ÎÊýΪ0x200,Èç¹ûΪ5~8,Ìî0x1FF //ÏêÇé¼ûC610µ÷ËÙÆ÷ÊÖ²á //½«µçÁ÷»òµçѹÊý¾Ý·¢¸øµç»ú //¸øµçÁ÷Ç°ÒªÏÈÅжϿª¹Ø״̬ÒÔ±ãÄܹ»¼°Ê±Í£³µ£¡£¡£¡ for(int i=0;i<1000;i++) { if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { break;//¼°Ê±Í£³µ»ò¼õËÙ } give_current[0]=PID_calc(&Pid[0],motor_data[0].speed_rpm,2000);//1ÂÖÍùÇ° give_current[1]=PID_calc(&Pid[1],motor_data[1].speed_rpm,-2000); //2ÂÖÍùÇ° give_current[2]=PID_calc(&Pid[2],motor_data[2].speed_rpm,-2000); //3ÂÖÍùÇ° give_current[3]=PID_calc(&Pid[3],motor_data[3].speed_rpm,2000);//4ÂÖÍùÇ° can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); } if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { continue;//¼°Ê±Í£³µ¼°Ê±Í£³µ»ò¼õËÙ } //Èç¹û²»ÊÇҪͣ³µ£¬ÄÇô¿ÉÒÔÈÃËùÓÐÂÖ×ÓÏÈͣһϣ¬ÕâÑù¿ÉÒÔ»³ö¸üºÃµÄ¾ØÐÎ for(int i=0;i<4;i++) { give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); } can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); osDelay(5); for(int i=0;i<1000;i++) { if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { break;//¼°Ê±Í£³µ»ò¼õËÙ } give_current[0]=PID_calc(&Pid[0],motor_data[0].speed_rpm,2000); //1ÂÖÍùÓÒ give_current[1]=PID_calc(&Pid[1],motor_data[1].speed_rpm,2000); //2ÂÖÍùÓÒ give_current[2]=PID_calc(&Pid[2],motor_data[2].speed_rpm,-2000); //3ÂÖÍùÓÒ give_current[3]=PID_calc(&Pid[3],motor_data[3].speed_rpm,-2000); //4ÂÖÍùÓÒ can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); } if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { continue;//¼°Ê±Í£³µ¼°Ê±Í£³µ»ò¼õËÙ } //Èç¹û²»ÊÇҪͣ³µ£¬ÄÇô¿ÉÒÔÈÃËùÓÐÂÖ×ÓÏÈͣһϣ¬ÕâÑù¿ÉÒÔ»³ö¸üºÃµÄ¾ØÐÎ for(int i=0;i<4;i++) { give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); } can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); osDelay(5); for(int i=0;i<1000;i++) { if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { break;//¼°Ê±Í£³µ»ò¼õËÙ } give_current[0]=PID_calc(&Pid[0],motor_data[0].speed_rpm,-2000); //1ÂÖÍùºó give_current[1]=PID_calc(&Pid[1],motor_data[1].speed_rpm,2000); //2ÂÖÍùºó give_current[2]=PID_calc(&Pid[2],motor_data[2].speed_rpm,2000); //3ÂÖÍùºó give_current[3]=PID_calc(&Pid[3],motor_data[3].speed_rpm,-2000); //4ÂÖÍùºó can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); } if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { continue;//¼°Ê±Í£³µ¼°Ê±Í£³µ»ò¼õËÙ } //Èç¹û²»ÊÇҪͣ³µ£¬ÄÇô¿ÉÒÔÈÃËùÓÐÂÖ×ÓÏÈͣһϣ¬ÕâÑù¿ÉÒÔ»³ö¸üºÃµÄ¾ØÐÎ for(int i=0;i<4;i++) { give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); } can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); osDelay(5); for(int i=0;i<1000;i++) { if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { break;//¼°Ê±Í£³µ»ò¼õËÙ } give_current[0]=PID_calc(&Pid[0],motor_data[0].speed_rpm,-2000); //1ÂÖÍù×ó give_current[1]=PID_calc(&Pid[1],motor_data[1].speed_rpm,-2000); //2ÂÖÍù×ó give_current[2]=PID_calc(&Pid[2],motor_data[2].speed_rpm,2000);//3ÂÖÍù×ó give_current[3]=PID_calc(&Pid[3],motor_data[3].speed_rpm,2000); //4ÂÖÍù×ó can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); } if(rc_ctrl.rc.s[0]==0x0001||rc_ctrl.rc.s[0]==0x0003) { continue;//¼°Ê±Í£³µ¼°Ê±Í£³µ»ò¼õËÙ } //Èç¹û²»ÊÇҪͣ³µ£¬ÄÇô¿ÉÒÔÈÃËùÓÐÂÖ×ÓÏÈͣһϣ¬ÕâÑù¿ÉÒÔ»³ö¸üºÃµÄ¾ØÐÎ for(int i=0;i<4;i++) { give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); } can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); osDelay(5); //×¢ÒâÒ»¶¨Òª¸øÑÓʱ£¬²»È»»á·è } }
根据受力分解分别计算处每条线路(前进或横移)小车的受力情况从而给4个电机配上对应的电流。
PS:0.注意电调ID要对应每个对应的轮子,这样计算的时候才不会出错。
并且注意电机的转动方向,对角轮的轮毂的滚子的朝向一致(最好是X型安装不要用O型,不然容易漂移)。但是同侧的电机的安装的朝向又是一致的,所以计算每个轮子在对应行程的时候还要考虑到电机的朝向。注意调整好转速值的正负值。(具体判断见下方三处)
1.多线程操作里面最好不要长时间的延时(最好不要达到秒级别),这可能导致进程直接卡死,就算没有卡死,也会影响其他线程的正常运行。
2.由于不知道小车每条线路的具体运行时间,所以可以使用For循环来反复的使用PID计算并附上相应电流值来保持小车有一定时间的向前或向右运行(循环次数越长,对应的时间耗时越久,虽然可以用官方的根据配置的系统时钟树的计算进程时间的函数,但是比较麻烦,所以才想着用for循环去找寻适当的循环次数来解决)。如果不加循环的话,那么给电流赋值就只有一次。这样1就不好对小车的行进进行控制了。更要命的是,如果在后面(每段行程的后面)还要加上几秒的延迟,那么电机极大会疯转,因为电机的CAN通信会以延时前的最后一次给到的电流去保持运行,但是根据PID原理,这最后一次电流赋值可能往往很大,所以电机很容易不受控。就比如给电机一定的转速,然后突然拔掉通信,那么电机极大可能以高速疯转。
3.需要在小车行进过程中判断开关状态以达到实时停车的效果(拉高停车的优先级),这样写保护的作用才能显现出来。不然当程序还在for循环里面跑,如果循环内不做判断,程序就会先跑完循环(对应小车的一段进程)才会去判断小车的开关,这样拨动开关去停车就会有”延迟“!并且注意for循环内进行判断用的是break,而for后面还要进行一次判断,并且用continue关键字,毕竟break只能击破一层循环。
然后就算其实for循环也可以用标志位来替代。因为外面还有一层while循环,所以可以对进入循环的次数进行累计,当达到阈值后切换状态(对应行进轨迹的改变),而这样就可以省略for循环的编写,并且这样就可以不用在for循环里面和后面分别去判断开关状态也能轻松时间停车功能了。
4.闭环控制。for循环内要反复计算当前转速并及时将新的电流值传给电机,而不要将计算转速的部分放在for循环的外面(前面),那样小车将不受控制。
三。摇杆控制小车运行
也是在之前的PID控制电机旋转的基础上进行修改。
关于每个轮子的速度计算:
v1 = 𝑣𝑥 + 𝑣𝑦 + 𝑤z v2 = 𝑣𝑥 − 𝑣𝑦 − 𝑤z v3 = 𝑣𝑥 + 𝑣𝑦 − 𝑤z v4 = 𝑣𝑥 − 𝑣𝑦 + 𝑤z
而由于当电机 1 和电机 4 处于前进速度的时候,电机 1 和电机 4 是顺时针旋转为负值, 电机 2 和电机 3 是逆时针旋转为正值,故而修正后的公式为:
v1 = -𝑣𝑥 - 𝑣𝑦 - 𝑤z v2 = 𝑣𝑥 − 𝑣𝑦 − 𝑤z v3 = 𝑣𝑥 + 𝑣𝑦 − 𝑤z v4 = -𝑣𝑥 − 𝑣𝑦 + 𝑤z
由于云台安装位置不在底盘中心,往往是在底盘前部,故而在旋转时,电机 1 和电机 2 需 要 较 慢 的 速 度 , 电 机 3 和 电 机 4 需 要 较快的速度,需要一个修正 因 子 CHASSIS_WZ_SET_SCALE(小于 1),假定为 a 参数,故而修改后的公式为:
v1 = -𝑣𝑥 - 𝑣𝑦 + (𝑎 − 1) ∗𝑤z v2 = 𝑣𝑥 − 𝑣𝑦 + (𝑎 − 1) ∗𝑤z v3 = 𝑣𝑥 + 𝑣𝑦 + (−𝑎 − 1) ∗ 𝑤z v4 = -𝑣𝑥 − 𝑣𝑦 + (−𝑎 − 1) ∗ 𝑤z
注意:官方的电调ID设置是逆时针设置的,也就是右上角是ID1,以上的计算公式是根据官方的电调ID来进行的计算,具体实现如果电调ID的官方的不一致,就要将对应的正负号进行修改。
然后对应的计算代码如下:
set_rpm[0]=(-rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]-rc_ctrl.rc.ch[0])*10; //1 set_rpm[1]=(rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]-rc_ctrl.rc.ch[0])*10; //2 set_rpm[2]=(rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]-rc_ctrl.rc.ch[0])*10; //3 set_rpm[3]=(-rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]-rc_ctrl.rc.ch[0])*10; //4 give_current[0]=PID_calc(&Pid[0],motor_data[0].speed_rpm,set_rpm[0]); //·¢ËÍÊý¾Ý¼ÆËãµçÁ÷ give_current[1]=PID_calc(&Pid[1],motor_data[1].speed_rpm,set_rpm[1]); //·¢ËÍÊý¾Ý¼ÆËãµçÁ÷ give_current[2]=PID_calc(&Pid[2],motor_data[2].speed_rpm,set_rpm[2]); //·¢ËÍÊý¾Ý¼ÆËãµçÁ÷ give_current[3]=PID_calc(&Pid[3],motor_data[3].speed_rpm,set_rpm[3]); //·¢ËÍÊý¾Ý¼ÆËãµçÁ÷ can_cmd_motor(&hcan1,0x200,give_current[0],give_current[1],give_current[2],give_current[3]); osDelay(5); //记得延时
由于摇杆通道3是左Y轴也就是控制小车前进或后退的部分,所以 rc_ctrl.rc.ch[3]对应Vx也就是朝向前后的部分,同理,rc_ctrl.rc.ch[2]对应Vy,rc_ctrl.rc.ch[0】对应右X并对应WZ也就是转动的部分,于是就实现了左边摇杆控制前后左右平移,右边摇杆控制自旋的功能,并且两个摇杆可以一起摇动来实现灵活的小车漂移。
PS:记得每次计算转速的时候还要乘上对应的系数去放大转速!