桌面计算机器,你的，桌面自平衡机器人—Zz

本帖最后由 Mingming.Zhang 于 2018-7-19 10:28 编辑

IMG_1084.jpg (58.05 KB, 下载次数: 25)

2018-6-10 10:51 上传

如 何 拥 有 一 台 足 够 把 玩 与 学 习 的 机 器 人 &＃xff1f;

它 集 2 轮 驱 动 、自 平 衡&＃xff0c;满 足 机 器 人 的 行 动&＃xff1b;

灰 度 巡 线、超 声 波 避 障、oled  128x64 的 图 形 显 示&＃xff1b;

mpu-6050 模 块 三 轴 加 速 度 &＃43; 三 轴 陀 螺 仪&＃xff0c;重 心 和 倾 斜 角 测 量&＃xff1b;

蓝 牙 无 线 连 接 操 控&＃xff1b;当 然 你 还 可 以 附 加 更 多 乐 趣 &＃xff0c;语 音 交 互、 微 型 摄 像 头 配 合 vr 头 盔(fpv 第 一 视 角 驾 驶)

小 小 的 身 躯 —— 清 新 不 油 腻

IMG_1085.jpg (66.81 KB, 下载次数: 16)

2018-6-10 10:55 上传

材  料  清  单 &＃xff1a;

————————————————

.n20带编码器金属齿轮减速电机V2 (30:1)

.miniQ小车橡胶轮

.3PI MiniQ N20电机支架

.FireBeetle原型扩展板

.TB6612FNG微型双路直流电机驱动模块

.DFRduino Pro Mini V1.2(16M5V328)

.3.7锂电池.3v转5v升压模块.锂电池充放电模块

———————————————————附加与扩展硬件

.HC-SR04&＃43;超声波测距模块

.灰度巡线传感器

硬  件  篇

9KP&＃96;W({$QFNUK9C&＃64;RQ7MM.jpg (48.08 KB, 下载次数: 21)

2018-6-15 10:25 上传

Board Power Supply 3.35 -12 V (3.3V model) or 5 - 12 V (5V model)

Circuit Operating Voltage 3.3V or 5V (depending on model)

8 MHz (3.3V versions) or 16 MHz (5V versions)

Serial: 0 (RX) and 1 (TX). Used to receive (RX) and transmit (TX) TTL serial data. These pins are connected to the TX-0 and RX-1 pins of the six pin header.

External Interrupts: 2 and 3.These pins can be configured to trigger an interrupt on a low value, a rising or falling edge, or a change in value. See the attachInterrupt function for details.

PWM: 3, 5, 6, 9, 10, and 11. Provide 8-bit PWM output with the analogWrite function.

SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK).These pins support SPI communication, which, although provided by the underlying hardware, is not currently included in the Arduino language.

pro mini作为一款非常小巧的微控制器&＃xff0c;大体了解到它的特性(蓝色字体标记)以及工作电压5v(供电是一个项目特别要考虑的指标)

5.jpg (80.42 KB, 下载次数: 19)

2018-6-15 10:37 上传

DSC09702.jpg (77.75 KB, 下载次数: 18)

2018-6-21 09:49 上传

DSC09709.jpg (76.09 KB, 下载次数: 20)

2018-6-21 09:49 上传Zz的底盘是一块洞洞板&＃xff0c;N20电机的输出轴露出洞洞板外(左右对称)&＃xff1b;

烧录进代码&＃xff1a;

[mw_shl_code&＃61;c,true]#include "./PinChangeInt.h"

#include "./MsTimer2.h"

//利用测速码盘计数实现速度PID控制

#include "./BalanceCar.h"

#include "./KalmanFilter.h"

//I2Cdev、MPU6050和PID_v1类库需要事先安装在Arduino 类库文件夹下

#include "./I2Cdev.h"

#include "./MPU6050_6Axis_MotionApps20.h"

#include "Wire.h"

MPU6050 mpu; //实例化一个 MPU6050 对象&＃xff0c;对象名称为 mpu

BalanceCar balancecar;

KalmanFilter kalmanfilter;

int16_t ax, ay, az, gx, gy, gz;

//驱动模块控制信号

#define IN1M A3

#define IN2M 7

#define IN3M A0

#define IN4M 8

#define PWMA A2

#define PWMB A1

#define STBY 6

#define PinA_left 3  //中断0

#define PinA_right 2 //中断1

//声明自定义变量

int time;

byte inByte; //串口接收字节

int num;

double Setpoint;                               //角度DIP设定点&＃xff0c;输入&＃xff0c;输出

double Setpoints, Outputs &＃61; 0;                         //速度DIP设定点&＃xff0c;输入&＃xff0c;输出

double kp &＃61; 40, ki &＃61; 0.1, kd &＃61; 0.3;                  //需要你修改的参数

double kp_speed &＃61;1.8, ki_speed &＃61; 0.05, kd_speed &＃61; 0;            // 需要你修改的参数

double kp_turn &＃61; 0, ki_turn &＃61; 0, kd_turn &＃61; 0;                 //旋转PID设定

const double PID_Original[6] &＃61; {38, 0.0, 0.58,4.0, 0.12, 0.0}; //恢复默认PID参数

//转向PID参数

double setp0 &＃61; 0, dpwm &＃61; 0, dl &＃61; 0; //角度平衡点&＃xff0c;PWM差&＃xff0c;死区&＃xff0c;PWM1&＃xff0c;PWM2

float value;

//********************angle data*********************//

float Q;

float Angle_ax; //由加速度计算的倾斜角度

float Angle_ay;

float K1 &＃61; 0.05; // 对加速度计取值的权重

float angle0 &＃61; 0.00; //机械平衡角

int slong;

//********************angle data*********************//

//***************Kalman_Filter*********************//

float Q_angle &＃61; 0.001, Q_gyro &＃61; 0.005; //角度数据置信度,角速度数据置信度

float R_angle &＃61; 0.5 , C_0 &＃61; 1;

float timeChange &＃61; 5; //滤波法采样时间间隔毫秒

float dt &＃61; timeChange * 0.001; //注意&＃xff1a;dt的取值为滤波器采样时间

//***************Kalman_Filter*********************//

//*********************************************

//******************** speed count ************

//*********************************************

volatile long count_right &＃61; 0;//使用volatile lon类型是为了外部中断脉冲计数值在其他函数中使用时&＃xff0c;确保数值有效

volatile long count_left &＃61; 0;//使用volatile lon类型是为了外部中断脉冲计数值在其他函数中使用时&＃xff0c;确保数值有效

int speedcc &＃61; 0;

//脉冲计算/

int lz &＃61; 0;

int rz &＃61; 0;

int rpluse &＃61; 0;

int lpluse &＃61; 0;

int sumam;

/脉冲计算

//转向、旋转参数///

int turncount &＃61; 0; //转向介入时间计算

float turnoutput &＃61; 0;

//转向、旋转参数///

//Wifi控制量///

#define run_car     &＃39;1&＃39;//按键前

#define back_car    &＃39;2&＃39;//按键后

#define left_car    &＃39;3&＃39;//按键左

#define right_car   &＃39;4&＃39;//按键右

#define stop_car    &＃39;0&＃39;//按键停

/*小车运行状态枚举*/

enum {

enSTOP &＃61; 0,

enRUN,

enBACK,

enLEFT,

enRIGHT,

enTLEFT,

enTRIGHT

} enCarState;

int incomingByte &＃61; 0;          // 接收到的 data byte

String inputString &＃61; "";         // 用来储存接收到的内容

boolean newLineReceived &＃61; false; // 前一次数据结束标志

boolean startBit  &＃61; false;  //协议开始标志

int g_carstate &＃61; enSTOP; //  1前2后3左4右0停止

String returntemp &＃61; ""; //存储返回值

boolean g_autoup &＃61; false;

int g_uptimes &＃61; 5000;

int front &＃61; 0;//前进变量

int back &＃61; 0;//后退变量

int turnl &＃61; 0;//左转标志

int turnr &＃61; 0;//右转标志

int spinl &＃61; 0;//左旋转标志

int spinr &＃61; 0;//右旋转标志

int bluetoothvalue;//蓝牙控制量

//蓝牙控制量///

//超声波速度//

int chaoshengbo &＃61; 0;

int tingzhi &＃61; 0;

int jishi &＃61; 0;

//超声波速度//

void Code_left();

void Code_right();

//脉冲计算///

void countpluse()

{

lz &＃61; count_left;

rz &＃61; count_right;

count_left &＃61; 0;

count_right &＃61; 0;

lpluse &＃61; lz;

rpluse &＃61; rz;

if ((balancecar.pwm1 <0) && (balancecar.pwm2 <0))                     //小车运动方向判断 后退时(PWM即电机电压为负) 脉冲数为负数

{

rpluse &＃61; -rpluse;

lpluse &＃61; -lpluse;

}

else if ((balancecar.pwm1 > 0) && (balancecar.pwm2 > 0))                 //小车运动方向判断 前进时(PWM即电机电压为正) 脉冲数为负数

{

rpluse &＃61; rpluse;

lpluse &＃61; lpluse;

}

else if ((balancecar.pwm1 <0) && (balancecar.pwm2 > 0))                 //小车运动方向判断 前进时(PWM即电机电压为正) 脉冲数为负数

{

rpluse &＃61; rpluse;

lpluse &＃61; -lpluse;

}

else if ((balancecar.pwm1 > 0) && (balancecar.pwm2 <0))               //小车运动方向判断 左旋转 右脉冲数为负数 左脉冲数为正数

{

rpluse &＃61; -rpluse;

lpluse &＃61; lpluse;

}

//提起判断

balancecar.stopr &＃43;&＃61; rpluse;

balancecar.stopl &＃43;&＃61; lpluse;

//每5ms进入中断时&＃xff0c;脉冲数叠加

balancecar.pulseright &＃43;&＃61; rpluse;

balancecar.pulseleft &＃43;&＃61; lpluse;

sumam &＃61; balancecar.pulseright &＃43; balancecar.pulseleft;

}

脉冲计算///

//角度PD

void angleout()

{

balancecar.angleoutput &＃61; kp * (kalmanfilter.angle &＃43; angle0) &＃43; kd * kalmanfilter.Gyro_x;//PD 角度环控制

}

//角度PD

//

//中断定时 5ms定时中断

/

void inter()

{

sei();

countpluse();                                     //脉冲叠加子函数

mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC获取MPU6050六轴数据 ax ay az gx gy gz

kalmanfilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);                                   //获取angle 角度和卡曼滤波

angleout();                                       //角度环 PD控制

speedcc&＃43;&＃43;;

if (speedcc >&＃61; 8)                                //40ms进入速度环控制

{

Outputs &＃61; balancecar.speedpiout(kp_speed, ki_speed, kd_speed, front, back, setp0);

speedcc &＃61; 0;

}

turncount&＃43;&＃43;;

if (turncount > 4)                                //40ms进入旋转控制

{

turnoutput &＃61; balancecar.turnspin(turnl, turnr, spinl, spinr, kp_turn, kd_turn, kalmanfilter.Gyro_z);                              //旋转子函数

turncount &＃61; 0;

}

balancecar.posture&＃43;&＃43;;

balancecar.pwma(Outputs, turnoutput, kalmanfilter.angle, kalmanfilter.angle6, turnl, turnr, spinl, spinr, front, back, kalmanfilter.accelz, IN1M, IN2M, IN3M, IN4M, PWMA, PWMB);                            //小车总PWM输出

}

//

//中断定时 5ms定时中断///

/

void SendAutoUp()

{

g_uptimes --;

if ((g_autoup &＃61;&＃61; true) && (g_uptimes &＃61;&＃61; 0))

{

//自动上报

String CSB, VT;

char temp[10]&＃61;{0};

float fgx;

float fay;

float leftspeed;

float rightspeed;

fgx &＃61; gx;  //传给局部变量

fay &＃61; ay;  //传给局部变量

leftspeed &＃61; balancecar.pwm1;

rightspeed &＃61; balancecar.pwm2;

double Gx &＃61; (double)((fgx - 128.1f) / 131.0f);              //角度转换

double Ay &＃61; ((double)fay / 16384.0f) * 9.8f;

if(leftspeed > 255 || leftspeed <-255)

return;

if(rightspeed > 255 || rightspeed <-255)

return;

if((Ay <-20) || (Ay > 20))

return;

if((Gx <-3000) || (Gx > 3000))

return;

returntemp &＃61; "";

memset(temp, 0x00, sizeof(temp));

//sprintf(temp, "%3.1f", leftspeed);

dtostrf(leftspeed, 3, 1, temp);  // 相當於 %3.2f

String LV &＃61; temp;

memset(temp, 0x00, sizeof(temp));

//sprintf(temp, "%3.1f", rightspeed);

dtostrf(rightspeed, 3, 1, temp);  // 相當於 %3.1f

String RV &＃61; temp;

memset(temp, 0x00, sizeof(temp));

//sprintf(temp, "%2.2f", Ay);

dtostrf(Ay, 2, 2, temp);  // 相當於 %2.2f

String AC &＃61; temp;

memset(temp, 0x00, sizeof(temp));

//sprintf(temp, "%4.2f", Gx);

dtostrf(Gx, 4, 2, temp);  // 相當於 %4.2f

String GY &＃61; temp;

CSB &＃61; "0.00";

VT &＃61; "0.00";

//AC &＃61;

returntemp &＃61; "$LV" &＃43; LV &＃43; ",RV" &＃43; RV &＃43; ",AC" &＃43; AC &＃43; ",GY" &＃43; GY &＃43; ",CSB" &＃43; CSB &＃43; ",VT" &＃43; VT &＃43; "#";

Serial.print(returntemp); //返回协议数据包

}

if (g_uptimes &＃61;&＃61; 0)

g_uptimes &＃61; 5000;

}

// &＃61;&＃61;&＃61;    初始设置     &＃61;&＃61;&＃61;

void setup() {

// TB6612FNGN驱动模块控制信号初始化

pinMode(IN1M, OUTPUT);                         //控制电机1的方向&＃xff0c;01为正转&＃xff0c;10为反转

pinMode(IN2M, OUTPUT);

pinMode(IN3M, OUTPUT);                        //控制电机2的方向&＃xff0c;01为正转&＃xff0c;10为反转

pinMode(IN4M, OUTPUT);

pinMode(PWMA, OUTPUT);                        //左电机PWM

pinMode(PWMB, OUTPUT);                        //右电机PWM

pinMode(STBY, OUTPUT);                        //TB6612FNG使能

//初始化电机驱动模块

digitalWrite(IN1M, 0);

digitalWrite(IN2M, 1);

digitalWrite(IN3M, 1);

digitalWrite(IN4M, 0);

digitalWrite(STBY, 1);

analogWrite(PWMA, 0);

analogWrite(PWMB, 0);

pinMode(PinA_left, INPUT);  //测速码盘输入

pinMode(PinA_right, INPUT);

// 加入I2C总线

Wire.begin();                            //加入 I2C 总线序列

Serial.begin(9600);                       //开启串口&＃xff0c;设置波特率为 115200

delay(1500);

mpu.initialize();                       //初始化MPU6050

delay(2);

balancecar.pwm1 &＃61; 0;

balancecar.pwm2 &＃61; 0;

//5ms定时中断设置  使用timer2    注意&＃xff1a;使用timer2会对pin3 pin11的PWM输出有影响&＃xff0c;因为PWM使用的是定时器控制占空比&＃xff0c;所以在使用timer的时候要注意查看对应timer的pin口。

MsTimer2::set(5, inter);

MsTimer2::start();

}

turn//

void ResetPID()

{

kp &＃61; PID_Original[0];

ki &＃61;  PID_Original[1];

kd &＃61;  PID_Original[2];                     //需要你修改的参数

kp_speed &＃61;  PID_Original[3];

ki_speed &＃61;  PID_Original[4];

kd_speed &＃61;  PID_Original[5];  // 需要你修改的参数

}

void ResetCarState()

{

turnl &＃61; 0;

turnr &＃61; 0;

front &＃61; 0;

back &＃61; 0;

spinl &＃61; 0;

spinr &＃61; 0;

turnoutput &＃61; 0;

}

// &＃61;&＃61;&＃61;       主循环程序体       &＃61;&＃61;&＃61;

void loop() {

String returnstr &＃61; "$0,0,0,0,0,0,0,0,0,0,0,0cm,8.2V#";  //默认发送

//主函数中循环检测及叠加脉冲 测定小车车速  使用电平改变既进入脉冲叠加 增加电机的脉冲数&＃xff0c;保证小车的精确度。

attachInterrupt(0, Code_left, CHANGE);

attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);

//kongzhi(); //蓝牙接口

//

//Serial.println(kalmanfilter.angle);

//Serial.print("\t");

//Serial.print(bluetoothvalue);

//Serial.print("\t");

//      Serial.print( balancecar.angleoutput);

//      Serial.print("\t");

//      Serial.print(balancecar.pwm1);

//      Serial.print("\t");

//      Serial.println(balancecar.pwm2);

//      Serial.print("\t");

//      Serial.println(balancecar.stopr);

//      Serial.print("\t");

if (newLineReceived)

{

switch (inputString[1])

{

case run_car:   g_carstate &＃61; enRUN;   break;

case back_car:  g_carstate &＃61; enBACK;  break;

case left_car:  g_carstate &＃61; enLEFT;  break;

case right_car: g_carstate &＃61; enRIGHT; break;

case stop_car:  g_carstate &＃61; enSTOP;  break;

default: g_carstate &＃61; enSTOP; break;

}

//判断协议是否有丢包

/* if(inputString.length() <21)

{

//恢复默认

inputString &＃61; "";   // clear the string

newLineReceived &＃61; false;

//Serial.print(returnstr);

goto a;

}*/

if (inputString[3] &＃61;&＃61; &＃39;1&＃39; && inputString.length() &＃61;&＃61; 21) //左摇

{

g_carstate &＃61; enTLEFT;

//Serial.print(returnstr);

}

else if (inputString[3] &＃61;&＃61; &＃39;2&＃39; && inputString.length() &＃61;&＃61; 21) //右摇

{

g_carstate &＃61; enTRIGHT;

// Serial.print(returnstr);

}

if (inputString[5] &＃61;&＃61; &＃39;1&＃39;) //查询PID

{

char charkp[7], charkd[7], charkpspeed[7], charkispeed[7];

dtostrf(kp, 3, 2, charkp);  // 相當於 %3.2f

dtostrf(kd, 3, 2, charkd);  // 相當於 %3.2f

dtostrf(kp_speed, 3, 2, charkpspeed);  // 相當於 %3.2f

dtostrf(ki_speed, 3, 2, charkispeed);  // 相當於 %3.2f

String strkp &＃61; charkp; String strkd &＃61; charkd; String strkpspeed &＃61; charkpspeed; String strkispeed &＃61; charkispeed;

returntemp &＃61; "$0,0,0,0,0,0,AP" &＃43; strkp &＃43; ",AD" &＃43; strkd &＃43; ",VP" &＃43; strkpspeed &＃43; ",VI" &＃43; strkispeed &＃43; "#";

Serial.print(returntemp); //返回协议数据包

}

else if (inputString[5] &＃61;&＃61; &＃39;2&＃39;) //恢复PID

{

ResetPID();

Serial.print("$OK#"); //返回协议数据包

}

if (inputString[7] &＃61;&＃61; &＃39;1&＃39;) //自动上报

{

g_autoup &＃61; true;

Serial.print("$OK#"); //返回协议数据包

}

else if (inputString[7] &＃61;&＃61; &＃39;2&＃39;) //停止自动上报

{

g_autoup &＃61; false;

Serial.print("$OK#"); //返回协议数据包

}

if (inputString[9] &＃61;&＃61; &＃39;1&＃39;) //角度环更新 $0,0,0,0,1,1,AP23.54,AD85.45,VP10.78,VI0.26#

{

int i &＃61; inputString.indexOf("AP");

int ii &＃61; inputString.indexOf(",", i);

if(ii > i)

{

String m_skp &＃61; inputString.substring(i &＃43; 2, ii);

m_skp.replace(".", "");

int m_kp &＃61; m_skp.toInt();

kp &＃61; (double)( (double)m_kp / 100.0f);

}

i &＃61; inputString.indexOf("AD");

ii &＃61; inputString.indexOf(",", i);

if(ii > i)

{

//ki &＃61; inputString.substring(i&＃43;2, ii);

String m_skd &＃61; inputString.substring(i &＃43; 2, ii);

m_skd.replace(".", "");

int m_kd &＃61; m_skd.toInt();

kd &＃61; (double)( (double)m_kd / 100.0f);

}

Serial.print("$OK#"); //返回协议数据包

}

if (inputString[11] &＃61;&＃61; &＃39;1&＃39;) //速度环更新

{

int i &＃61; inputString.indexOf("VP");

int ii &＃61; inputString.indexOf(",", i);

if(ii > i)

{

String m_svp &＃61; inputString.substring(i &＃43; 2, ii);

m_svp.replace(".", "");

int m_vp &＃61; m_svp.toInt();

kp_speed &＃61; (double)( (double)m_vp / 100.0f);

}

i &＃61; inputString.indexOf("VI");

ii &＃61; inputString.indexOf("#", i);

if(ii > i)

{

String m_svi &＃61; inputString.substring(i &＃43; 2, ii);

m_svi.replace(".", "");

int m_vi &＃61; m_svi.toInt();

ki_speed &＃61; (double)( (double)m_vi / 100.0f);

Serial.print("$OK#"); //返回协议数据包

}

}

//恢复默认

inputString &＃61; "";   // clear the string

newLineReceived &＃61; false;

}

a:    switch (g_carstate)

{

case enSTOP: turnl &＃61; 0; turnr &＃61; 0;  front &＃61; 0; back &＃61; 0; spinl &＃61; 0; spinr &＃61; 0; turnoutput &＃61; 0; break;

case enRUN: ResetCarState();front &＃61; 250; break;

case enLEFT: turnl &＃61; 1; break;

case enRIGHT: turnr &＃61; 1; break;

case enBACK: ResetCarState();back &＃61; -250; break;

case enTLEFT: spinl &＃61; 1; break;

case enTRIGHT: spinr &＃61; 1; break;

default: front &＃61; 0; back &＃61; 0; turnl &＃61; 0; turnr &＃61; 0; spinl &＃61; 0; spinr &＃61; 0; turnoutput &＃61; 0; break;

}

//增加自动上报

SendAutoUp();

}

pwm///

//脉冲中断计算/

void Code_left() {

count_left &＃43;&＃43;;

} //左测速码盘计数

void Code_right() {

count_right &＃43;&＃43;;

} //右测速码盘计数

//脉冲中断计算/

//serialEvent()是IDE1.0及以后版本新增的功能&＃xff0c;不清楚为什么大部份人不愿意用&＃xff0c;这个可是相当于中断功能一样的啊!

int num1 &＃61; 0;

void serialEvent()

{

while (Serial.available())

{

incomingByte &＃61; Serial.read();              //一个字节一个字节地读&＃xff0c;下一句是读到的放入字符串数组中组成一个完成的数据包

if (incomingByte &＃61;&＃61; &＃39;$&＃39;)

{

num1 &＃61; 0;

startBit &＃61; true;

}

if (startBit &＃61;&＃61; true)

{

num1&＃43;&＃43;;

inputString &＃43;&＃61; (char) incomingByte;     // 全双工串口可以不用在下面加延时&＃xff0c;半双工则要加的//

}

if (startBit &＃61;&＃61; true && incomingByte &＃61;&＃61; &＃39;#&＃39;)

{

newLineReceived &＃61; true;

startBit &＃61; false;

}

if(num1 >&＃61; 80)

{

num1 &＃61; 0;

startBit &＃61; false;

newLineReceived &＃61; false;

inputString &＃61; "";

}

}

}

/*备份*/

#if 0

char chartemp[7];

dtostrf(ax, 3, 2, chartemp);  // 相當於 %3.2f

String strax &＃61; chartemp;

strax &＃61; "\nax:" &＃43; strax;

memset(chartemp, 0x00, 7);

dtostrf(ay, 3, 2, chartemp);  // 相當於 %3.2f

String stray &＃61; chartemp;

stray &＃61; "\nay:" &＃43; stray;

memset(chartemp, 0x00, 7);

dtostrf(az, 3, 2, chartemp);  // 相當於 %3.2f

String straz &＃61; chartemp;

straz &＃61; "\naz:" &＃43; straz;

memset(chartemp, 0x00, 7);

dtostrf(gx, 3, 2, chartemp);  // 相當於 %3.2f

String strgx &＃61; chartemp;

strgx &＃61; "\ngx:" &＃43; strgx;

memset(chartemp, 0x00, 7);

dtostrf(gy, 3, 2, chartemp);  // 相當於 %3.2f

String strgy &＃61; chartemp;

strgy &＃61; "\ngy:" &＃43; strgy;

memset(chartemp, 0x00, 7);

dtostrf(gz, 3, 2, chartemp);  // 相當於 %3.2f

String strgz &＃61; chartemp;

strgz &＃61; "\ngz:" &＃43; strgz;

Serial.print(strax &＃43; stray &＃43; straz &＃43; strgx &＃43; strgy &＃43; strgz); //返回协议数据包

#endif

[/mw_shl_code]

完成&＃xff01;