开源平衡车/zh

概述 项目名称：Microduino开源平衡车 目的：DIY一台属于自己的两轮自平衡小车 难度：中 耗时：2小时 制作者： 简介: 两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本教程将指导读者基于Microduino产品模块组建一个可以遥控的自平衡小车，能够实现两轮小车平衡直立和运动的功能，用户在制作完成后，可以在该平台上进行二次开发，实现更多有趣功能。 材料清单 Microduino设备 模块 数量 功能 Microduino-Core+/zh 1 核心板 Microduino-USBTTL/zh 1 下载程序 Microduino-Module nRF/zh 1 无线通信 Microduino-Module Motion/zh 1 检测姿态 Microduino-Shield Stepper/zh 1 驱动连接地板 其他设备 模块 数量 功能 2.4G天线 1 2.4G通讯 固定板材 1 固定支撑 M2尼龙螺丝 4 固定底板 M2尼龙螺柱 4 固定底板 M2尼龙螺母 4 固定底板 短铜柱 8 固定电机 M4金属螺丝 2 固定车轮 2s锂电池（7.4V） 1 供电 Micro-USB数据线 1 串口通信，下载程序 车轴连接器 2 连接电机轴和车轮 车轮 2 结构 步进电机 2 驱动车轮 步进电机线 2 连接电机和驱动板 1000px 程序下载 从以下地址下载源程序 BalanceCar_Microduino 下载程序 堆叠模块 将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），再通过USB数据线把Microduino-USBTTL模块与电脑连接起来。 配置环境 打开Arduino IDE for Microduino环境，（搭建参考：AVR核心：Getting started/zh） 点击 【文件】->【打开】 浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开 500px 600px 点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，点击“”按钮下载程序到核心板上。 600px 平衡车搭建 Step1 按图1-1所示将结构-A1和结构-A2对接 按图1-2所示将结构-B1和结构-B2插在结构-A1两边 步骤1完成后组成小车骨架 Step2 按图2-1所示将2个步进电机与结构-C1对接，再用铜柱固定好 注意：注意图中所示电机接口方向，两个电机接口应该对称安装。 步骤2完成后组成电机组件 Step3 按图3-1将电机组件卡到小车骨架上，注意电机接口需要对应小车骨架的缺口，请按图中所示方向将电机组件卡入卡槽内 电机组件卡到位后，将结构-B1卡入卡槽缺口，卡紧电机组件 350px 步骤3完成后组成小车地盘 250px Step4 按图4-1所示将车轴连接器固定在步进电机轴上，然后将车轮装在车轴连接器上，然后用螺丝固定紧 250px 步骤4完成后小车如图4-2所示 250px Step5 按图5-1所示将2s电池放在小车地盘的槽内,然后将结构-C2插在小车地盘上面，将结构-C3插在小车地盘的底部 250px 按图5-2说是用结构-A3、结构-C4和结构-C5卡紧小车上的部件 250px 步骤5完成后小车如图5-3所示 250px Step6 将Microduino-Shield Stepper/zh用尼龙螺丝、尼龙螺柱和尼龙螺母固定在小车地盘顶部 将2.4G天线接到Microduino-Module nRF/zh上 然后将Microduino-Core+/zh,Microduino-Module nRF/zh和Microduino-Module Motion/zh叠加到Microduino-Shield Stepper/zh上 250px 然后按以下示意图用步进电机线连接步进电机和Microduino-Shield Stepper/zh，请注意两个步进电机线的接口位置。 250px 注意：在连接电机线的时候要注意方向，并稍微用力才能插入电机的接口，请按照图中方向插接。 200px Step7 按图7-1所示将2s电池插入Microduino-Shield Stepper/zh的电池接口，小车即上电工作，扶起小车，小车即能保持平衡不倒 Joypad遥控指南 搭建过程可参考以下页面：Joypad使用指南 Joypad通讯模式配置* 模式配置可参考以下页面：通讯模式配置 当我们使用Microduino-Module nRF/zh作为通讯模块时，Joypad对应选择nRF模式即可 Joypad电池使用说明* 特别要注意的是电池使用说明：电池使用说明 代码说明 在“user_def.h”文件是我们的配置文件 以下代码可以配置nRF模式下通道，需要保证和Joypad遥控器一致 Joypad的nRF模式通道配置可参考：nRF模式通道配置 #define NRF_CHANNEL 70 //nRF通道 以下代码可以配置油门和转向对应通道 对应通道说明可参考：通道/操作说明 #define CHANNEL_THROTTLE 2 //油门通道 #define CHANNEL_STEERING 1 //转向通道 以下代码可以修正两轮的转速比例 值得范围是-1到1之间 设置成-1，是最大转速比反转 设置成1，是最大转速比正转， 如果小车不能走直线，应该将较慢的一边轮子的转速比例值降低 #define motor_fixL 1 //速度修正 -1到1之间 #define motor_fixR 1 //速度修正 -1到1之间 注意问题 下载程序时候最好只叠加core（core+）和USBTTL，虽然本次搭建涉及的nRF24不会引起冲突，但是别的通信模块有时会造成串口冲突，养成好习惯。 Core+要叠在nRF24，USB的底下，紧贴ROBOT板。 锂电池正负极别接错了，否则会烧坏电路。 调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池 平衡车程序说明 nrf设置部分 //nRF============================== SPI.begin(); //初始化SPI总线 radio.begin(); //此处与Joypad上设定的通道数对应 network.begin(/*channel*/ 70, /*node address*/ this_node); digitalWrite(LED, HIGH); Serial.println("===========start==========="); 端口设定 #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 与遥控器进行数据交换的数组 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; }; PID控制实现 // 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); } PID算法-平衡控制 void robot() { //=============================================================== timer_value = millis(); // 新的DMP定位解决方案 fifoCount = mpu.getFIFOCount(); if (fifoCount >= 18) { if (fifoCount > 18) // 如果我们有一个以上的数据包，我们采取简单的路径：丢弃缓冲区 { mpu.resetFIFO(); return; } loop_counter++; 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? target_angle = 10; //我们整合输出（加速度） 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?) if ((angle_adjusted < 66) && (angle_adjusted > -66)) { if (node_STA) // pushUp mode? { pushUp_counter++; if (pushUp_counter > 60) // 0.3 seconds { // 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; } } } 遥控器数据接收后选择数据控制平衡车的前后与左右 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); //接收请求时序赋值 Joypad程序及说明 Joypad部分 def.h中 定义了 uint8_t nrf_channal = 70; //0~125 nrf_channal为nrf通信的通道，joypad和Cube小车的代码中都会有该定义 当通道一致时则Joypad可与Cube小车成功连接。 在小车代码中会有如下程序段 //nRF============================== SPI.begin(); //初始化SPI总线 radio.begin(); network.begin(/*channel*/ 70 , /*node address*/ this_node); 在data.h中 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); 8位数组outBuf表示Joypad发出的8位数据，0位为右摇杆左右，1为右摇杆上下，2为左摇杆左右，3位左摇杆上下,4~7位对应AUX0~4 在nrf.h中 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; }; 此处定义的send_a结构体位对应的0位要发送的数据 视频