第七天：打造自己的遥控小车

目录

一：安装底盘

二：完成小车全自动画矩形

1.使用带有PID计算和遥控器的工程（最好同时使用了FreeRTOS多线程任务），在原有代码的基础修改。

2.写保护！

3.画矩形

三。摇杆控制小车运行

---

一：安装底盘

安装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：记得每次计算转速的时候还要乘上对应的系数去放大转速！