|
|
| （未显示8个用户的116个中间版本） |
| 第1行： |
第1行： |
| − | {| style="width: 800px;"
| + | === 详细内容 === |
| − | |-
| |
| − | |
| |
| − | ==概述== | |
| − | *项目名称：Microduino开源平衡车
| |
| − | *目的：制作一台可以用遥控板控制的自平衡机器人小车
| |
| − | *难度：中
| |
| − | *耗时：2小时
| |
| − | *制作者：
| |
| − | *简介:
| |
| − | 两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本次教程我们使用Microduino产品模块快速搭建一个可以用遥控板控制的自平衡机器人小车，玩家可以迅速上手，并且看到小车运动和平衡的效果，玩家们可以在制作结束后，继续更深一步的智能控制部分的研究。
| |
| − | ==材料清单== | |
| − | *Microduino设备
| |
| − | {|class="wikitable"
| |
| − | |-
| |
| − | |模块||数量||功能
| |
| − | |-
| |
| − | |[[Microduino-Core+/zh]]||1||核心板
| |
| − | |-
| |
| − | |[[Microduino-USBTTL/zh]] ||1||下载程序
| |
| − | |-
| |
| − | |[[Microduino-nRF24/zh]] ||1||无线通信
| |
| − | |-
| |
| − | |[[Microduino-Shield Stepper/zh]] ||1||驱动连接地板
| |
| | | | |
| − | |}
| + | 此部分内容维基现已停止维护，该部分的详细内容全部转移至IdeaLab网站上，请移步[https://www.idealab.cc/course/5a69513f8459d000199e6cca/class/5a56ec78d6bcf1000f49fca7 开源平衡车——制作]上进行查看。以下是IdeaLab访问流程。 |
| − | *其他设备
| |
| − | {|class="wikitable"
| |
| − | |-
| |
| − | |模块||数量||功能
| |
| − | |-
| |
| − | |2.4G天线||1||2.4G通讯
| |
| − | |-
| |
| − | |固定支架||1||固定支撑
| |
| − | |-
| |
| − | |尼龙螺丝||4||固定
| |
| − | |-
| |
| − | |尼龙螺母||12||固定
| |
| − | |-
| |
| − | |电池盒 ||1||装载电池
| |
| − | |-
| |
| − | |电池 ||2||供电
| |
| − | |-
| |
| − | |Micro-USB数据线 ||1||串口通信，下载程序
| |
| − | |-
| |
| − | |车轴 ||2||连接车轮
| |
| − | |-
| |
| − | |车轮子 ||2||结构
| |
| − | |-
| |
| − | |步进电机 ||2||驱动车轮
| |
| − | |}
| |
| − | *Joypad材料
| |
| − | {|class="wikitable"
| |
| − | |-
| |
| − | |模块||数量||功能
| |
| − | |-
| |
| − | |[[Microduino-Core]]||1||核心
| |
| − | |-
| |
| − | |[[Microduino-nRF24]]||1||无线通讯
| |
| − | |-
| |
| − | |[[Microduino-Joypad]]||1||遥控器
| |
| − | |-
| |
| − | |[[Microduino-TFT]]||1||显示
| |
| − | |-
| |
| − | |[[Microduino-USBTTL]]||1||下载程序
| |
| − | |-
| |
| − | |锂电池 ||1||供电
| |
| − | |-
| |
| − | |亚克力面板||1||面板
| |
| − | |-
| |
| − | |尼龙柱 ||4||固定
| |
| − | |-
| |
| − | |长尼龙螺丝 ||4||固定
| |
| − | |-
| |
| − | |短尼龙螺丝 ||4||固定
| |
| − | |-
| |
| − | |尼龙螺母 ||4||固定
| |
| − | |-
| |
| − | |海绵板 ||1||固定
| |
| − | |}
| |
| − | [[File:平衡车物料.jpg||1000px|center]]
| |
| | | | |
| − | ==实验原理== | + | == IdeaLab访问流程 == |
| − | *PID原理
| + | '''1、打开网址（[http://www.idealab.cc/ https://www.idealab.cc/]）。''' |
| − | PID在自动控制领域有着极其重要的作用，作为最早实用化的控制技术已经有70多年的历史，近几年一些创客们自制的一些很酷的东西，如：四轴飞行器，自平衡车等等都离不开它的身影。
| |
| − | 首先了解什么是PID。PID实指“比例proportional”、“积分integral”、“微分derivative”，如果我们要求被控制的对象最终趋于一个稳定的结果，一般就可以使用PID算法。
| |
| − | 假设说，有一辆速度为1m/s的小车，我们要求他的速度改变为5m/s，要完成这样的一件事，我们必须要有：
| |
| − | *小车驱动装置（用程序控制它输出多大的电压，电压决定驱动的马力）；
| |
| − | *被驱动器控制的部分（即小车）；
| |
| − | *检测当前速度的装置（当前速度与目标速度的差称为误差）；
| |
| − | 本来，我们可以给小车一个驱动力让小车加速，直到检测到小车速度达到5m/s，撤去驱动力。但是，这样做会带来几个问题：
| |
| − | 1)当小车速度达到5m/s时，从装置检测到这个速度，通知控制器，让控制器改变输出的电压，这一个过程需要耗费一定时间，在这个时间里面，小车速度可能增加了不少。
| |
| − | 2)撤去驱动力后，外界条件如摩擦会让小车速度进一步改变。
| |
| − | 然而，PID算法可以在一定误差内解决这些问题。
| |
| − | 使用PID算法时，大致是这样的。每一个采样周期，通过速度检测装置获得当前速度，传入程序，通过程序计算得到电压控制小车得到新速度。下一个采样周期又把新速度传入，获得新电压，再传入速度，再获得电压，如此反复。 PID算法的关键，是如何根据当前得到的速度值，输出一个“恰当”的电压，以致小车最终能够趋于稳定。
| |
| − | PID算法采用比例，积分，微分(Proportion Integral Differential)三种方法进行控制。三种方法都有自己对应的一个常量(pconst，iconst，dconst)。这三个变量都需要在实验中多次尝试得出。用数学公式表达PID算法如下图所示：
| |
| − | 此处：e = 期望值 – 输入值
| |
| − | [[File:PIDtheory.jpg||600px|center]] | |
| − | 平衡车之所以可以自己掌握平衡，首先通过Microduino-10DOF模块的加速度和陀螺仪测出相应的姿态数据，然后利用kalman滤波算法得出当前平衡车的角度。平衡的角度是180度，如果测出的角度偏离180，PID算法会调整输出相应的PWM值给电机，从而保持小车平衡。
| |
| − | PID原理有点像锅炉房里烧锅炉，首先定下来锅炉的恒定温度，比如26摄氏度，锅炉房里的墙上有一个温度计，能够实时测得锅炉的实时温度。锅炉房里通常有个老大爷时不时（每十分钟看一次）的看着墙上的温度计，如果温度高了就给锅炉降温，低了就给锅炉升温。如果让一个没有经验的年轻小伙子来管理锅炉的温度，可以想象温度表的值会上下浮动的，有经验的老大爷会把这个浮动降到最低。其实PID就是这个烧锅炉的例子，在代码中就有这个故事的影子：
| |
| − | 1)规定的26度就是setpoint
| |
| − | 2)当前的温度就是CurrentAngle
| |
| − | 3)实际值与26度的偏差就是error
| |
| − | 4)没有经验的小伙子有点像PID中的P
| |
| − | 5)有经验的老大爷相当于PID了
| |
| − | 6)每十分钟看一次相当于PID计算的周期时间
| |
| − | PID的主要代码：
| |
| − | <source lang="cpp">
| |
| − | // PD的实施。 DT是毫秒
| |
| − | float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
| |
| − | {
| |
| − | float error;
| |
| − | float output;
| |
| − | error = setPoint - input;
| |
| − | // Kd的两部分实施
| |
| − | //仅使用输入（传感器）的一部分而不是设定值输入（T-2）最大的一个
| |
| − | //而第二个使用该设定值
| |
| − | output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
| |
| − | //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
| |
| − | PID_errorOld2 = PID_errorOld;
| |
| − | PID_errorOld = input; // 误差为Kd值是唯一的输入组件
| |
| − | setPointOld = setPoint;
| |
| − | return(output);
| |
| − | }
| |
| − | //P控制实现。
| |
| − | float speedPControl(float input, float setPoint, float Kp)
| |
| − | {
| |
| − | float error;
| |
| − | error = setPoint - input;
| |
| − | return(Kp * error);
| |
| − | }
| |
| − | </source>
| |
| − | 本套件利用陀螺仪和加速度传感器（Microduino-10DOF/zh）来检测车体态的变化，并利用步进电机控制核心（Microduino-Stepper/zh），精确地驱动电机进行相应的调整，以保持系统的平衡。
| |
| − | [[File:PIDtheory1.jpg||600px|center]]
| |
| − | *主要传感器
| |
| − | [[Microduino-10DOF]]
| |
| − | ==文档==
| |
| − | 百度盘地址:
| |
| − | http://pan.baidu.com/s/1eQBaMjg
| |
| − | 提取码：h08a
| |
| | | | |
| | + | '''2、点击右上角登录按钮。''' |
| | | | |
| − | github地址：
| + | [[文件:1.png]] |
| − | [https://github.com/wasdpkj/BalanceCar_Microduino/tree/master/BalanceCar_Microduino BalanceCar_Microduino] | |
| | | | |
| − | ==程序下载==
| + | '''3、登录/注册账号。''' |
| − | 将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），通过USB数据线与电脑连接起来
| |
| | | | |
| − | 确认你搭建了Microduino的开发环境，否则参考附录1-Arduino IDE安装指导。
| + | [[文件:2.png]] |
| − | [[File:Gettingstarted.jpg||500px|center]] | |
| − | 打开Arduino IDE编程软件，点击 【文件】->【打开】
| |
| − | [[File:Dl1.jpg||600px|center]]
| |
| − | 浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开
| |
| − | [[File:Balancecaropen1.jpg||600px|center]]
| |
| − | [[File:Balancecaropen2.jpg||600px|center]]
| |
| − | 点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序
| |
| − | [[File:WiFiStationopen4.jpg||600px|center]]
| |
| | | | |
| − | ==平衡车搭建==
| + | '''4、点击课程，在课程列表中找到《Microduino创新编程》课程并点击打开。''' |
| − | *'''Step1''':首先将A1和A2对接,然后将B1和B2插在A1两端
| |
| − | [[File:Balancestep1.jpg||600px|center]]
| |
| − | *'''Step2''':将电机与C1对接，再用螺丝帽固定好
| |
| − | [[File:Balancestep2.jpg||600px|center]]
| |
| − | *'''Step3''':将第一步与第二部的结果拼接在一起,用B1进行固定
| |
| − | [[File:Balancestep3.jpg||600px|center]]
| |
| − | *'''Step4''':将轮胎用螺丝帽和螺丝固定在电机上
| |
| − | [[File:Balancestep4.jpg||600px|center]]
| |
| − | *'''Step5''':将电池模块放在图中指定位置,分别用C2和C3接在对应位置,用C4,A3,C5固定
| |
| − | [[File:Balancestep5.jpg||600px|center]]
| |
| − | *'''Step6''':将Core+,USBTTL,10DOF和底板进行拼接再用螺丝和螺丝帽固定好,最后使用传感器线和电池接入如图位置.
| |
| − | [[File:Balancestep6.jpg||600px|center]]
| |
| − | 注意：在连接电机线的时候要注意方向，并稍微用力才能插入电机的接口，请按照图中方向插接。
| |
| − | [[File:Balancemotor.jpg||200px|center]]
| |
| − | ====Joypad搭建====
| |
| − | *'''Step 1'''：将Microduino-TFT从Microduino-Joypad面板后面卡进Microduino-Joypad面板上，用尼龙螺丝固定，注意Microduino-TFT安装方向
| |
| − | [[File:Joypadstep1.jpg|center|600px]]
| |
| − | '''注意：接不同的电池需要拨动中间的开关，在图中已有标志。需要先拨开关再接入电池，否则会影响使用。'''
| |
| − | [[File:Joypadstep1_1.jpg|center|800px]]
| |
| − | *'''Step 2'''：将传感器接线插在Microduino-TFT的接口上
| |
| − | [[File:Joypadstep2.jpg|center|600px]]
| |
| − | *'''Step 3'''：首先分别将两个摇杆按键、电池、四个白色按键放入对应位置，之后将连接好天线的nRF模块和Core装到Joypad底板上
| |
| − | [[File:Joypadstep3.jpg|center|600px]]
| |
| − | *'''Step 4'''：将Microduino-TFT传感器接线的另一头接到底板上的相应位置，之后将长版螺丝帽放到四个角的相应位置
| |
| − | [[File:Joypadstep4.jpg|center|600px]]
| |
| − | *'''Step 5'''：将Joypad的表壳和底板使用螺丝和螺丝帽固定好
| |
| − | [[File:Joypadstep5.jpg|center|600px]]
| |
| − | *'''Step 6'''：组装完成后将天线上的贴纸撕下，将天线贴在底板背面的任意位置，至此Joypad组装完毕
| |
| − | [[File:Joypadstep6.jpg|center|600px]]
| |
| − | *自平衡小车和Joypad测试
| |
| | | | |
| − | ====Joypad调试====
| + | [[文件:8.png]] |
| − | *按键对应
| |
| − | 在打开Joypad之后的4秒左右时间之内按下Key1(下方最左侧的按键)，会进入设置(Config)模式
| |
| − | [[File:Step1进入设置.jpg|600px|center|]] | |
| − | *进入设置模式
| |
| − | 按照图中的颜色，从左至右对应为Key1~Key4
| |
| − | [[File:Step1按键对应.jpg|600px|center|]]
| |
| − | 注意：必须在进入操作界面前进入（4S左右时间）。若未进入则重启进入'''
| |
| − | *摇杆校准
| |
| − | 按动Key3和Key4使光标上下移动，Key1为返回，Key2为确认
| |
| − | 选择第一项Joystick Config进入摇杆设置模式
| |
| − | 继续选择Joystick Correct进入摇杆校准模式。
| |
| − | 进入之后会显示如图中第三张图所示的界面，初始状态为两个十字
| |
| − | 此时摇动左右摇杆至最上，最下，最左，最右四个极限状态
| |
| − | （推荐操作方式：将摇杆摇动一圈）
| |
| − | 摇动之后会看到十字的四个方向出现圆圈，圆圈扩大到最大状态证明已经是摇杆的极限位置
| |
| − | 校准之后按Key2确认并返回上一页面
| |
| − | [[File:Step2摇杆校准.jpg|600px|center|]]
| |
| − | *选择控制模式
| |
| − | 按Key1回到主界面，选择第二项Protocol Config进入模式选择
| |
| − | 选择第一项Mode，之后选择nRF24即robot控制模式，按下Key2确认并返回
| |
| − | [[File:Step3设置Robot模式.jpg|600px|center|]]
| |
| − | *设置通信信道
| |
| − | 返回二级菜单，选择nRF24 Channel按下Key2确认
| |
| − | 选择70，它是与Robot_Microduino.ino中nRF24的配置函数设置相对应的
| |
| − | [[File:Step4通信通道设置robot.jpg|600px|center|]]
| |
| | | | |
| − | ==注意问题==
| + | '''5、点击“报名学习”。''' |
| − | *下载程序时候最好只叠加core（core+）和USBTTL，虽然本次搭建涉及的nRF24不会引起冲突，但是别的通信模块有时会造成串口冲突，养成好习惯。
| |
| − | *Core+要叠在nRF24，USB的底下，紧贴ROBOT板。
| |
| − | *锂电池正负极别接错了，否则会烧坏电路。
| |
| − | *调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池
| |
| − | ==平衡车程序说明==
| |
| − | nrf设置部分
| |
| − | <source lang="cpp">
| |
| − | //nRF==============================
| |
| − | SPI.begin(); //初始化SPI总线
| |
| − | radio.begin();
| |
| − | //此处与Joypad上设定的通道数对应
| |
| − | network.begin(/*channel*/ 70, /*node address*/ this_node);
| |
| | | | |
| − | digitalWrite(LED, HIGH);
| + | [[文件:9.png]] |
| − | Serial.println("===========start===========");
| |
| − | </source>
| |
| − | 端口设定
| |
| − | <source lang = "cpp">
| |
| − | #define MOTOR_EN 4 //PORTB,0
| |
| − | #define MOTOR1_DIR A0 //PORTA,7
| |
| − | #define MOTOR1_STEP 5 //PORTB,1
| |
| − | #define MOTOR2_DIR A1 //PORTA,6
| |
| − | #define MOTOR2_STEP 6 //PORTB,2
| |
| − | #define MOTOR3_DIR A2 //PORTA,5
| |
| − | #define MOTOR3_STEP 7 //PORTB,3
| |
| − | #define MOTOR4_DIR A3 //PORTA,4
| |
| − | #define MOTOR4_STEP 8 //PORTD,6
| |
| − | </source>
| |
| − | 与遥控器进行数据交换的数组
| |
| − | <source lang="cpp">
| |
| − | struct receive_a //接收
| |
| − | {
| |
| − | uint32_t ms;
| |
| − | uint16_t rf_CH0;
| |
| − | uint16_t rf_CH1;
| |
| − | uint16_t rf_CH2;
| |
| − | uint16_t rf_CH3;
| |
| − | uint16_t rf_CH4;
| |
| − | uint16_t rf_CH5;
| |
| − | uint16_t rf_CH6;
| |
| − | uint16_t rf_CH7;
| |
| − | };
| |
| − | </source>
| |
| − | PID控制实现
| |
| − | <source lang="cpp">
| |
| − | // PD的实施。 DT是毫秒
| |
| − | float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
| |
| − | {
| |
| − | float error;
| |
| − | float output;
| |
| − | error = setPoint - input;
| |
| − | // Kd的两部分实施
| |
| − | //仅使用输入（传感器）的一部分而不是设定值输入输入（T-2）最大的一个
| |
| − | //而第二个使用该设定值，使之更有点侵略性设定点设定点（T-1）
| |
| − | output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; // + 错误 - PID_error_Old2
| |
| − | //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
| |
| − | PID_errorOld2 = PID_errorOld;
| |
| − | PID_errorOld = input; // 误差为Kd值是唯一的输入组件
| |
| − | setPointOld = setPoint;
| |
| − | return (output);
| |
| − | }
| |
| − | //P控制实现。
| |
| − | float speedPControl(float input, float setPoint, float Kp)
| |
| − | {
| |
| − | float error;
| |
| − | error = setPoint - input;
| |
| − | return (Kp * error);
| |
| − | }
| |
| − | // PI实现。 DT是毫秒
| |
| − | float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki)
| |
| − | {
| |
| − | float error;
| |
| − | float output;
| |
| − | error = setPoint - input;
| |
| − | PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
| |
| − | PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);
| |
| − | output = Kp * error + Ki * PID_errorSum * DT * 0.001;
| |
| − | return (output);
| |
| − | }
| |
| − | </source>
| |
| − | PID算法-平衡控制
| |
| − | <source lang="cpp">
| |
| − | void robot()
| |
| − | {
| |
| − | //===============================================================
| |
| − | timer_value = millis();
| |
| | | | |
| − | // 新的DMP定位解决方案
| + | '''6、输入姓名。''' |
| − | fifoCount = mpu.getFIFOCount();
| |
| − | if (fifoCount >= 18)
| |
| − | {
| |
| − | if (fifoCount > 18) // 如果我们有一个以上的数据包，我们采取简单的路径：丢弃缓冲区
| |
| − | {
| |
| − | mpu.resetFIFO();
| |
| − | return;
| |
| − | }
| |
| | | | |
| − | loop_counter++;
| + | [[文件:5.png]] |
| − | slow_loop_counter++;
| |
| − | dt = (timer_value - timer_old);
| |
| − | timer_old = timer_value;
| |
| − | angle_adjusted_Old = angle_adjusted;
| |
| − | angle_adjusted = dmpGetPhi() + ANGLE_FIX;
| |
| − | Serial.println(angle_adjusted);
| |
| − | mpu.resetFIFO(); // 我们始终复位FIFO
| |
| − | // 我们计算估计机器人的速度
| |
| − | actual_robot_speed_Old = actual_robot_speed;
| |
| − | actual_robot_speed = (speed_m[1] - speed_m[0]) / 2; // 正面：前锋
| |
| − | // 角速度角度调整角度调整旧
| |
| − | int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0;
| |
| − | // 我们利用机器人速度（T-1）或（T-2），以补偿该延迟
| |
| − | int16_t estimated_speed = actual_robot_speed_Old - angular_velocity;
| |
| − | //估计速度估计过滤滤速
| |
| − | estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05;
| |
| − | //目标角速度PIC ONTROL dt的速度估计过滤油门Kp_thr Ki_thr
| |
| − | target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
| |
| − | //有限的输出 目标角度约束目标角度最大目标角度最大目标角度
| |
| − | target_angle = constrain(target_angle, -max_target_angle, max_target_angle);
| |
| | | | |
| − | if (pushUp_counter > 0) // pushUp mode?
| + | '''7、点击“课程目录”。''' |
| − | target_angle = 10;
| |
| | | | |
| − | //我们整合输出（加速度）
| + | [[文件:10.png]] |
| − | control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
| |
| − | control_output = constrain(control_output, -800, 800); // 限制最大输出控制
| |
| − | // 控制的转向部分的输出直接注射
| |
| − | motor1 = control_output + steering;
| |
| − | motor2 = -control_output + steering; //马达2反转
| |
| − | // 限制最大速度
| |
| − | motor1 = constrain(motor1, -500, 500);
| |
| − | motor2 = constrain(motor2, -500, 500);
| |
| | | | |
| − | // Is robot ready (upright?)
| + | '''8、选择相应课程进行学习。''' |
| − | if ((angle_adjusted < 66) && (angle_adjusted > -66))
| |
| − | {
| |
| − | if (node_STA) // pushUp mode?
| |
| − | {
| |
| − | pushUp_counter++;
| |
| | | | |
| − | if (pushUp_counter > 60) // 0.3 seconds
| + | [[文件:11.png]] |
| − | {
| |
| − | // Set motors to 0 => disable steppers => robot
| |
| − | setMotorSpeed(0, 0);
| |
| − | setMotorSpeed(1, 0);
| |
| − | // We prepare the raiseup mode
| |
| − | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
| |
| − | Kd = KD_RAISEUP;
| |
| − | Kp_thr = KP_THROTTLE_RAISEUP;
| |
| − | control_output = 0;
| |
| − | estimated_speed_filtered = 0;
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | setMotorSpeed(0, motor1);
| |
| − | setMotorSpeed(1, motor2);
| |
| − | }
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | // NORMAL MODE
| |
| − | setMotorSpeed(0, motor1);
| |
| − | setMotorSpeed(1, motor2);
| |
| − | pushUp_counter = 0;
| |
| − | }
| |
| − | | |
| − | if ((angle_adjusted < 40) && (angle_adjusted > -40))
| |
| − | {
| |
| − | Kp = Kp_user; // Default or user control gains
| |
| − | Kd = Kd_user;
| |
| − | Kp_thr = Kp_thr_user;
| |
| − | Ki_thr = Ki_thr_user;
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
| |
| − | Kd = KD_RAISEUP;
| |
| − | Kp_thr = KP_THROTTLE_RAISEUP;
| |
| − | Ki_thr = KI_THROTTLE_RAISEUP;
| |
| − | }
| |
| − | }
| |
| − | else // Robot not ready, angle > 70º
| |
| − | {
| |
| − | setMotorSpeed(0, 0);
| |
| − | setMotorSpeed(1, 0);
| |
| − | PID_errorSum = 0; // Reset PID I term
| |
| − | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
| |
| − | Kd = KD_RAISEUP;
| |
| − | Kp_thr = KP_THROTTLE_RAISEUP;
| |
| − | Ki_thr = KI_THROTTLE_RAISEUP;
| |
| − | }
| |
| − | | |
| − | }
| |
| − | }
| |
| − | </source>
| |
| − | 遥控器数据接收后选择数据控制平衡车的前后与左右
| |
| − | <source lang="cpp">
| |
| − | float * _i = _speed;
| |
| − | //CH1,CH0,CH7对应之前的发送/接收部分定义的数组
| |
| − | _i[0] = map(rec.rf_CH1, 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE);
| |
| − | _i = _turn;
| |
| − | _i[0] = map(rec.rf_CH0, 1000, 2000, -MAX_STEERING, MAX_STEERING);
| |
| − | | |
| − | node_STA = (rec.rf_CH7 > 1500 ? true : false); //接收请求时序赋值
| |
| − | </source>
| |
| − | | |
| − | ==Joypad程序及说明==
| |
| − | Joypad部分
| |
| − | def.h中
| |
| − | 定义了
| |
| − | <source lang = "cpp">
| |
| − | uint8_t nrf_channal = 70; //0~125
| |
| − | </source>
| |
| − | nrf_channal为nrf通信的通道，joypad和Cube小车的代码中都会有该定义
| |
| − | 当通道一致时则Joypad可与Cube小车成功连接。
| |
| − | 在小车代码中会有如下程序段
| |
| − | <source lang = "cpp">
| |
| − | //nRF==============================
| |
| − | SPI.begin(); //初始化SPI总线
| |
| − | radio.begin();
| |
| − | network.begin(/*channel*/ 70 , /*node address*/ this_node);
| |
| − | </source>
| |
| − | 在data.h中
| |
| − | <source lang = "cpp">
| |
| − | outBuf[0] = Joy1_x;
| |
| − | outBuf[1] = Joy1_y;
| |
| − | outBuf[2] = Joy_x;
| |
| − | outBuf[3] = Joy_y;
| |
| − | outBuf[4] = map(AUX[0], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | outBuf[5] = map(AUX[1], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | outBuf[6] = map(AUX[2], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | outBuf[7] = map(AUX[3], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | </source>
| |
| − | 8位数组outBuf表示Joypad发出的8位数据，0位为右摇杆左右，1为右摇杆上下，2为左摇杆左右，3位左摇杆上下,4~7位对应AUX0~4
| |
| − | | |
| − | 在nrf.h中
| |
| − | <source lang = "cpp">
| |
| − | struct send_a //发送
| |
| − | {
| |
| − | uint32_t ms;
| |
| − | uint16_t rf_CH0;
| |
| − | uint16_t rf_CH1;
| |
| − | uint16_t rf_CH2;
| |
| − | uint16_t rf_CH3;
| |
| − | uint16_t rf_CH4;
| |
| − | uint16_t rf_CH5;
| |
| − | uint16_t rf_CH6;
| |
| − | uint16_t rf_CH7;
| |
| − | };
| |
| − | </source>
| |
| − | 此处定义的send_a结构体位对应的0位要发送的数据
| |
| − | | |
| − | ==视频==
| |
