亲，“电路城论坛”已合并升级到更全、更大、更强的「新与非网」。了解「新与非网」

论坛

 注册/登录  
返回列表 发新帖
查看: 560|回复: 0

[经验] C#获取维特智能WT901C-485姿态传感器的X Y Z角度

[复制链接]
winnner

该用户从未签到
发表于 2019-8-16 11:10:49 | 显示全部楼层 |阅读模式
分享到：
1.WT901C-485姿态传感器的控制和数据协议说明
1 、读取指令和数据回传格式












  • xyz轴角度所在寄存器地址：






2 拼接获取xyz轴角度指令：

0x50  0x03  0x00  0x3d  0x00  0x3e  0x00  0x3f  0x00  0x33  0x00  0x003

3 编写代码

3.1 查看串口

win10环境直接在左下输入设备管理进入设备管理器查看设备所在串口

3.2 建立 wpf 项目——modbus













3.3 串口代码Comm.cs

using System;

using System.Collections.Generic;

using System.IO.Ports;

using System.Linq;

using System.Text;

using System.Threading;

using System.Threading.Tasks;

using System.Windows;



namespace Modbus

{

    public class Comm

    {

        public delegate void EventHandle(byte[] readBuffer);

        public event EventHandle DataReceived;



        public SerialPort serialPort;

        Thread thread;

        volatile bool _keepReading;



        public Comm()

        {

            serialPort = new SerialPort();

            thread = null;

            _keepReading = false;

        }



        public bool IsOpen

        {

            get

            {

                return serialPort.IsOpen;

            }

        }



        private void StartReading()

        {

            if (!_keepReading)

            {

                _keepReading = true;

                thread = new Thread(new ThreadStart(ReadPort));

                thread.Start();

            }

        }



        private void StopReading()

        {

            if (_keepReading)

            {

                _keepReading = false;

                thread.Join();

                thread = null;

            }

        }



        private void ReadPort()

        {

            while (_keepReading)

            {

                if (serialPort.IsOpen)

                {

                    int count = serialPort.BytesToRead;

                    if (count > 0)

                    {

                        byte[] readBuffer = new byte[count];

                        try

                        {

                            //Application.DoEvents();

                            serialPort.Read(readBuffer, 0, count);

                            if (DataReceived != null)

                                DataReceived(readBuffer);

                            Thread.Sleep(100);

                        }

                        catch (TimeoutException)

                        {

                        }

                    }

                }

            }

        }



        public void Open()

        {

            Close();

            try

            {

                serialPort.Open();

            }

            catch (Exception e)

            {

            }

            if (serialPort.IsOpen)

            {

                StartReading();

            }

            else

            {

                MessageBox.Show("串口打开失败！");

            }

        }



        public void Close()

        {

            StopReading();

            serialPort.Close();

        }



        public void WritePort(byte[] send, int offSet, int count)

        {

            if (IsOpen)

            {

                serialPort.Write(send, offSet, count);

            }

        }

    }

}

3.4 modbus协议代码 单一指令 获取xyz

using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using System.Threading.Tasks;



namespace Modbus

{

    //读取从机数据

    class modbus

    {

        private const byte devAddr = 0x50;//设备地址，默认

        private const byte funCode = 0x03;//读功能码



        private const byte regHxH = 0x00;

        private const byte regHxL = 0x3d;//x轴角度

        private const byte regHyH = 0x00;

        private const byte regHyL = 0x3e;//y轴角度

        private const byte regVzH = 0x00;

        private const byte regVzL = 0x3f;//z轴角度



        private const byte regNumH = 0x00;

        private const byte regNumL = 0x03;//寄存器个数

        private const byte CRCH = 0x00;

        private const byte CRCL = 0x00;





        public byte[] getXYZ() {

            return new byte[] { devAddr,funCode, regHxH, regHxL,regNumH, regNumL, CRCH, CRCL };

        }

    }

}

​

3.5 界面查询按钮触发获取xyz

<Window x:Class="Modbus.MainWindow"

        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"

        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"

        Title="MainWindow" Height="350" Width="525">

    <Grid>

        <Button Content="查询" HorizontalAlignment="Left" VerticalAlignment="Top" Width="75" Margin="10,117,0,0" Click="select"/>

        <Label Content="x" HorizontalAlignment="Left" Margin="119,24,0,0" VerticalAlignment="Top"/>

        <Label Content="y" HorizontalAlignment="Left" Margin="258,24,0,0" VerticalAlignment="Top" Width="19"/>

        <Label Content="z" HorizontalAlignment="Left" Margin="396,24,0,0" VerticalAlignment="Top" RenderTransformOrigin="2.625,0.32" Width="18"/>

        <TextBlock  x:Name="x" HorizontalAlignment="Left" Margin="150,24,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="97"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock  x:Name="y" HorizontalAlignment="Left" Margin="277,24,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="114"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock  x:Name="z" HorizontalAlignment="Left" Margin="424,24,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="90"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <Label Content="水平x" x:Name="hX" HorizontalAlignment="Left" Margin="106,168,0,0" VerticalAlignment="Top" Width="43" Height="24"/>

        <Label Content="水平y" x:Name="hY" HorizontalAlignment="Left" Margin="241,168,0,0" VerticalAlignment="Top" Width="43" Height="24"/>

        <Label Content="垂直z" x:Name="vZ" HorizontalAlignment="Left" Margin="386,168,0,0" VerticalAlignment="Top" Width="43" Height="24"/>

        <TextBlock HorizontalAlignment="Left" Margin="149,167,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="97"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock HorizontalAlignment="Left" Margin="284,167,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="97"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock HorizontalAlignment="Left" Margin="424,167,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="90"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBox x:Name="recv" HorizontalAlignment="Left" Height="44" Margin="173,67,0,0" TextWrapping="Wrap" Text="TextBox" VerticalAlignment="Top" Width="284"/>

        <Label Content="recv" HorizontalAlignment="Left" Margin="119,67,0,0" VerticalAlignment="Top" RenderTransformOrigin="0.938,2.04" Height="23" Width="49"/>



    </Grid>

</Window><Window x:Class="Modbus.MainWindow"

        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"

        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"

        Title="MainWindow" Height="350" Width="525">

    <Grid>

        <Button Content="查询" HorizontalAlignment="Left" VerticalAlignment="Top" Width="75" Margin="10,117,0,0" Click="select"/>

        <Label Content="x" HorizontalAlignment="Left" Margin="119,24,0,0" VerticalAlignment="Top"/>

        <Label Content="y" HorizontalAlignment="Left" Margin="258,24,0,0" VerticalAlignment="Top" Width="19"/>

        <Label Content="z" HorizontalAlignment="Left" Margin="396,24,0,0" VerticalAlignment="Top" RenderTransformOrigin="2.625,0.32" Width="18"/>

        <TextBlock  x:Name="x" HorizontalAlignment="Left" Margin="150,24,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="97"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock  x:Name="y" HorizontalAlignment="Left" Margin="277,24,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="114"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock  x:Name="z" HorizontalAlignment="Left" Margin="424,24,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="25" Width="90"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <Label Content="水平x"  HorizontalAlignment="Left" Margin="106,168,0,0" VerticalAlignment="Top" Width="43" Height="24"/>

        <Label Content="水平y"  HorizontalAlignment="Left" Margin="241,168,0,0" VerticalAlignment="Top" Width="43" Height="24"/>

        <Label Content="垂直z"  HorizontalAlignment="Left" Margin="386,168,0,0" VerticalAlignment="Top" Width="43" Height="24"/>

        <TextBlock x:Name="hX" HorizontalAlignment="Left" Margin="149,167,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="69" Width="97"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock x:Name="hY" HorizontalAlignment="Left" Margin="284,167,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="69" Width="97"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBlock x:Name="vZ" HorizontalAlignment="Left" Margin="424,167,0,0" TextWrapping="Wrap" VerticalAlignment="Top" RenderTransformOrigin="0.927,0.267" Height="69" Width="90"><Run Language="zh-cn" Text="1111"/></TextBlock>

        <TextBox x:Name="recv" HorizontalAlignment="Left" Height="44" Margin="173,67,0,0" TextWrapping="Wrap" Text="TextBox" VerticalAlignment="Top" Width="284"/>

        <Label Content="recv" HorizontalAlignment="Left" Margin="119,67,0,0" VerticalAlignment="Top" RenderTransformOrigin="0.938,2.04" Height="23" Width="49"/>





    </Grid>

</Window>

using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using System.Threading.Tasks;

using System.Windows;

using System.Windows.Controls;

using System.Windows.Data;

using System.Windows.Documents;

using System.Windows.Input;

using System.Windows.Media;

using System.Windows.Media.Imaging;

using System.Windows.Navigation;

using System.Windows.Shapes;





namespace Modbus

{

    /// <summary>

    /// MainWindow.xaml 的交互逻辑

    /// </summary>

    public partial class MainWindow : Window

    {

        private Comm comm;

        private modbus modbus = new modbus();

        public MainWindow()

        {

            InitializeComponent();

            initComPort("COM4");

        }





        private void initComPort(String portName)

        {

            comm = new Comm();

            comm.serialPort.PortName = portName;

            //波特率

            comm.serialPort.BaudRate = 9600;

            //数据位

            comm.serialPort.DataBits = 8;

            //两个停止位

            comm.serialPort.StopBits = System.IO.Ports.StopBits.One;

            //无奇偶校验位

            comm.serialPort.Parity = System.IO.Ports.Parity.None;

            comm.serialPort.ReadTimeout = 100;

            comm.serialPort.WriteTimeout = -1;





            comm.Open();

            if (comm.IsOpen)

            {

                comm.DataReceived += new Comm.EventHandle(comm_DataReceived);

            }

        }





        void comm_DataReceived(byte[] readBuffer)

        {

            if (readBuffer[0] == 80) return;

            if (readBuffer[0] == 3)

            {

                StringBuilder pp = new StringBuilder();

                for (int i = 0; i < readBuffer.Length; i++)

                {

                    pp.Append("0x" + readBuffer.ToString("X2") + " ");

                }

                Array.Reverse(readBuffer, 2, 2);

                int x = BitConverter.ToInt16(readBuffer, 2);





                Array.Reverse(readBuffer, 4, 2);

                int y = BitConverter.ToInt16(readBuffer, 4);





                Array.Reverse(readBuffer, 6, 2);

                int z = BitConverter.ToInt16(readBuffer, 6);





                double angleX = x* 180.0 / 32768.0;

                double angleY = y * 180.0 / 32768;

                double angleZ = z * 180.0 / 32768.0;

                //更新界面数据

               Dispatcher.BeginInvoke(new Action(() =>

               {

                   this.recv.Text = pp.ToString() + "\n";





                   this.hX.Text = angleX.ToString();

                   this.hY.Text = angleY.ToString();

                   this.vZ.Text = angleZ.ToString();





                   this.x.Text = x.ToString();

                   this.y.Text = y.ToString();

                   this.z.Text = z.ToString();

               }));

            }

            }

        }

        private void select(object sender, RoutedEventArgs e)

        {

            byte[] contol = modbus.getXYZ();

            comm.WritePort(contol, 0, contol.Length);

        }

    }

}

3.6 代码解释说明

程序初始化，实例化串口，打开串口。点击select按钮触发事件，向comm口发送控制指令getXYZ，获取xyz轴角度。comm口的DataReceived收到传感器数据后，调用函数comm_DataReceive.

comm_DataReceive解析回传数据

数据回传第一个包为0x50,表示设备地址，忽略。

第二个包格式 0x03 0x06 ... ... ...  

06表示有6个寄存器回传数据，这里是x,y,z.每个数据占两个寄存器，分高低位。具体转换办法如下：

（官方说明）：
输出数据的正负号是按照补码的方式表示的，也就是其二进制数据的最高位如果为1则表示负数。程序编写的时候，可以采用强制转化为有符号的short类型来解决符号的问题，具体做法是，将数据的高位强制DataH转化为short类型，然后再左移8位，和低字节DataL进行与操作。例如角速度包的解析方法：

角速度包一共有11个字节，chrTemp[11]，其中chrTemp[3]为X轴角速度的高8位，chrTemp[2]为X轴角速度的低8位，那么角速度的解析代码如下：

float w[0];

w[0]=(( ((short)chrTemp[3])«8)|chrTemp[2])/32768*2000;

其中( ((short)chrTemp[3])«8)|chrTemp[2]得到short类型的有符号数据，short类型的数据表示范围是-32768~32767之间，角速度的量程范围是正负2000°，所以需要除以32768再乘以2000。这样处理以后，得到的数据就是有符号的float类型数据了。

这里转换x轴的代码，y,z相同。

(((short)readBuffer[2] << 8) | readBuffer[3]) * 180 / 32768;

有些小问题，这个代码计算出来的都是整数，不是确切值。需要的自己改啊，哈哈哈

3.7 修改

经过实际测试(((short)readBuffer[2] << 8) | readBuffer[3]) * 180 / 32768，只在值为正数的时候才是正确的值，与传感器回传值不符合。上面代码已经修改为：

Array.Reverse(readBuffer, 2, 2);



int x = BitConverter.ToInt16(readBuffer, 2);

把回传值高低位转换后利用c#的API ToInt16直接读取两个字节进行转换，得到的是有符号的10进制数据。

计算角度的时候，采用下面在数据后面加0的方式得到double型数据，然后根据需要截取数据的精度。

double angleX = x* 180.0 / 32768.0;

3.8 实际运行效果图：













​

附加说明：
实际z轴经过校准之后是直接可以表示与北方夹角。




版权声明：本文为CSDN博主「春风十里不如你9527」的原创文章，遵循CC 4.0 by-sa版权协议，转载请附上原文出处链接及本声明。



下载资料失效了？点击此处告知管理员 > >
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 注册/登录

本版积分规则

关闭

站长推荐上一条 /2 下一条

手机版|小黑屋|与非网

GMT+8, 2024-4-20 13:29 , Processed in 0.107720 second(s), 14 queries , MemCache On.

ICP经营许可证 苏B2-20140176  苏ICP备14012660号-2   苏州灵动帧格网络科技有限公司 版权所有.

苏公网安备 32059002001037号

Powered by Discuz! X3.4

Copyright © 2001-2020, Tencent Cloud.