using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using RoBoIO_DotNet;
namespace controller_AX12
{
public partial class Form1 : Form
{
public byte[] ax01 = new byte[11]{ 0xff, 0xff, 0x0f, 0x07, 0x03, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00 };
public byte[] ax02 = new byte[11]{ 0xff, 0xff, 0x12, 0x07, 0x03, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00 };
public byte[] read = new byte[6];
public int run;
public Form1()
{
InitializeComponent();
groupBox1.Enabled = false;
checkBox1.Enabled = false;
run = 0;
}
private void Form1_FormClosing(object sender, FormClosingEventArgs e)
{
//RoBoIO.com3_DisableTurboMode();
RoBoIO.com3_Close();
}
private void Start_Click(object sender, EventArgs e)
{
if (!groupBox1.Enabled){
if (!RoBoIO.roboio_SetRBVer(RoBoIO.RB_100RD))
MessageBox.Show("roboio_SetRBVer fail");
if (!RoBoIO.com3_Init(RoBoIO.COM_HDUPLEX))
MessageBox.Show("com3_Init fail");
if (!RoBoIO.com3_SetFormat(RoBoIO.COM_BYTESIZE8, RoBoIO.COM_STOPBIT1, RoBoIO.COM_NOPARITY))
MessageBox.Show("com3_SetFormat fail");
if (!RoBoIO.com3_SetBaud(RoBoIO.COMBAUD_115200BPS))
MessageBox.Show("com3_SetBaud fail");
//RoBoIO.com3_EnableTurboMode();
groupBox1.Enabled = true;
checkBox1.Enabled = true;
Start.Enabled = false;
ax02[10] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7] + ax02[8] + ax02[9]))[0];
ax01[10] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7] + ax01[8] + ax01[9]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 11, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 11, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
ax01[3] = 0x05;
ax01[5] = 0x20;
ax02[3] = 0x05;
ax02[5] = 0x20;
}
}
private void button1_Click(object sender, EventArgs e)
{
xUp();
}
private void xUp() {
int speed = 10;
if (run != 0)
{
Stop(5);
speed = 5;
}
int v = speedbar.Value;
run = 1;
for (int i = 0; i < v % 0x3ff; i = i + v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & (i + 0x400))[0];
ax01[7] = BitConverter.GetBytes((0xff00 & (i + 0x400)) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax02[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
}
private void button3_Click(object sender, EventArgs e)
{
xdown();
}
private void xdown() {
int speed = 10;
if (run != 0)
{
Stop(5);
speed = 5;
}
int v = speedbar.Value;
run = 2;
for (int i = 0; i < v % 0x3ff; i = i + v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax01[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & (i + 0x400))[0];
ax02[7] = BitConverter.GetBytes((0xff00 & (i + 0x400)) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
}
private void button2_Click(object sender, EventArgs e)
{
xleft();
}
private void xleft() {
int speed = 10;
if (run != 0)
{
Stop(5);
speed = 5;
}
int v = speedbar.Value;
run = 3;
for (int i = 0; i < v % 0x3ff; i = i + v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & (i + 0x400))[0];
ax01[7] = BitConverter.GetBytes((0xff00 & (i + 0x400)) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & (i + 0x400))[0];
ax02[7] = BitConverter.GetBytes((0xff00 & (i + 0x400)) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
}
private void button4_Click(object sender, EventArgs e)
{
xright();
}
private void xright() {
int speed = 10;
if (run != 0)
{
Stop(5);
speed = 5;
}
int v = speedbar.Value;
run = 4;
for (int i = 0; i < v % 0x3ff; i = i + v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax01[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax02[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
}
private void button5_Click(object sender, EventArgs e)
{
Stop(10);
}
private void Stop(int speed)
{
if (run != 0)
{
int v = speedbar.Value;
switch (run)
{
case 1:
for (int i = v % 0x3ff; i > 0; i = i - v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & (i + 0x3ff))[0];
ax01[7] = BitConverter.GetBytes((0xff00 & (i + 0x3ff)) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax02[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
break;
case 2:
for (int i = v % 0x3ff; i > 0; i = i - v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax01[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & (i + 0x3ff))[0];
ax02[7] = BitConverter.GetBytes((0xff00 & (i + 0x3ff)) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
break;
case 3:
for (int i = v % 0x3ff; i > 0; i = i - v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & (i + 0x3ff))[0];
ax01[7] = BitConverter.GetBytes((0xff00 & (i + 0x3ff)) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & (i + 0x3ff))[0];
ax02[7] = BitConverter.GetBytes((0xff00 & (i + 0x3ff)) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
break;
case 4:
for (int i = v % 0x3ff; i > 0; i = i - v / speed)
{
ax01[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax01[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[6] = BitConverter.GetBytes(0x00ff & i)[0];
ax02[7] = BitConverter.GetBytes((0xff00 & i) / 0xff)[0];
ax02[8] = BitConverter.GetBytes(~(ax02[2] + ax02[3] + ax02[4] + ax02[5] + ax02[6] + ax02[7]))[0];
ax01[8] = BitConverter.GetBytes(~(ax01[2] + ax01[3] + ax01[4] + ax01[5] + ax01[6] + ax01[7]))[0];
if (!RoBoIO.com3_ServoTRX(ax01, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
if (!RoBoIO.com3_ServoTRX(ax02, 9, read, 6))
MessageBox.Show("com3_ServoTRX fail read back = " + read[0] + " " + read[1] + " " + read[2] + " " + read[3] + " " + read[4] + " " + read[5]);
RoBoIO.delay_ms(25);
}
break;
}
run = 0;
}
}
private void checkBox1_CheckedChanged(object sender, EventArgs e)
{
if (checkBox1.Checked)
{
groupBox1.Enabled = false;
}
else {
groupBox1.Enabled = true;
}
}
private void checkBox1_KeyDown(object sender, KeyEventArgs e)
{
if (checkBox1.Checked)
{
if (e.KeyCode == Keys.W && run != 1)
xUp();
else if (e.KeyCode == Keys.S && run != 2)
xdown();
else if (e.KeyCode == Keys.A && run != 3)
xleft();
else if (e.KeyCode == Keys.D && run != 4)
xright();
}
}
private void checkBox1_KeyUp(object sender, KeyEventArgs e)
{
if(checkBox1.Checked){
if (run != 0)
Stop(10);
}
}
}
}