//ROV控制部分**********************************************************************************************************
#region
private void button1_Click(object sender, EventArgs e)//打开串口ROV
{
if (!ROV.IsOpen)
{
ROV.PortName = comboBox_ROV.Text; //设置串口名
ROV.BaudRate = 9600; //设置默认波特率
try
{
ROV.Open(); //打开串口
button1.Text = "关闭串口";
comboBox_ROV.Enabled = false; //关闭使能
ROV.DataReceived += new SerialDataReceivedEventHandler(ROV_DataReceived); //串口接收处理函数
}
catch
{
MessageBox.Show("ROV串口打开失败！");
}
}
else
{
try
{
ROV.Close(); //关闭串口
button1.Text = "打开串口";
comboBox_ROV.Enabled = true; //打开使能
}
catch
{
MessageBox.Show("ROV串口关闭失败！");
}
}
}
private void LedBtn_MouseUp(object sender, MouseEventArgs e)
{
ROV rov = robotThread.Robot;
if (rov != null)
{
rov.Led.Enabled = false;
}
}
private void ServoSelector_DropDown(object sender, EventArgs e)
{
ServoSelector.Items.Clear();
ROV robot = thread.Robot;
if (robot != null)
{
foreach (KeyValuePair <string, Servo> servo in robot.Servos)
{
ServoSelector.Items.Add(new ServoWrapper(servo.Value, servo.Key));
}
}
}
private void ThrusterSelector_DropDown(object sender, EventArgs e)
{
ThrusterSelector.Items.Clear();
ROV robot = thread.Robot;
if (robot != null)
{
foreach (KeyValuePair <string, Thruster> thruster in robot.Thrusters)
{
ThrusterSelector.Items.Add(new ThrusterWrapper(thruster.Value, thruster.Key));
}
}
}
private void ROV_DataReceived(object sender, SerialDataReceivedEventArgs e)//显示ROV返回的信息
{
try
{
string str = ROV.ReadExisting(); //字符串方式读
textBox1.AppendText(str); //添加文本
textBox1.ScrollToCaret(); //自动显示至最后行
ROV_control_order(); //不断向ROV发指令，避免点击太快响应跟不上
}
catch (Exception exp)
{
MessageBox.Show(exp.Message);
}
}
private void ROV_control_order()
{
textBox1.Focus();//点击按钮后让textBox1获得焦点，避免焦点落在按钮上使键盘失效
string order = "#" + order_convert(label7.Text) + order_convert(label8.Text) + order_convert(label9.Text) + order_convert(label10.Text) + "$";
//发送数据
if (ROV.IsOpen)
{ //如果串口开启
ROV.Write(order); //写数据
}
else
{
MessageBox.Show("串口未打开");
}
}
private void Timer1_Tick(object sender, EventArgs e)
{
ROV rov = robotThread.Robot;
TestBtnMeter.Value = rov?.Button0?.State ?? false;
TestBtn2.Value = rov?.Button1?.State ?? false;
TempLabel.Text = "Temperature: " + ((rov == null) ? "----" : rov.IMU.Temperature.ToString().PadLeft(4)) + "°C";
AccelVectorPanel.Vector = rov?.IMU?.Accelerometer ?? new Vector3();
MagVectorPanel.Vector = rov?.IMU?.Magnetometer ?? new Vector3();
GyroVectorPanel.Vector = rov?.IMU?.Gyroscope ?? new Vector3();
EulerVectorPanel.Vector = rov?.IMU?.Euler ?? new Vector3();
LinearVectorPanel.Vector = rov?.IMU?.LinearAccel ?? new Vector3();
GravityVectorPanel.Vector = rov?.IMU?.Gravity ?? new Vector3();
Quaternion quat = rov?.IMU?.Quaternion ?? new Quaternion();
QuatVectorPanel.Vector = new Vector4(quat.X, quat.Y, quat.Z, quat.W);
WaterTempLabel.Text = "Water Temp: " + ((rov == null) ? "----------" : rov.PressureSensor.Temperature.ToString("0.00").PadLeft(10)) + "°C";
PressureLabel.Text = "Pressure: " + ((rov == null) ? "----------" : rov.PressureSensor.Pressure.ToString("0.00").PadLeft(10)) + " mBar";
AltitudeLabel.Text = "Altitude: " + ((rov == null) ? "----------" : rov.PressureSensor.Altitude.ToString("0.00").PadLeft(10)) + " m above mean sea";
DepthLabel.Text = "Depth: " + ((rov == null) ? "----------" : rov.PressureSensor.Depth.ToString("0.00").PadLeft(10)) + " m";
}
private void HardwarePingToolStripMenuItem_Click(object sender, EventArgs e)
{
ROV robot = robotThread.Robot;
if (robot == null)
{
MessageBox.Show("No robot connected!");
}
else
{
Log.Info("Pinging...");
long?timeMs = robot.Ping(1000);
if (timeMs == null)
{
Log.Info("Ping failed.");
MessageBox.Show("Ping failed.");
}
else
{
Log.Info("Ping responded in " + ((long)timeMs).ToString() + " ms.");
MessageBox.Show("Ping: " + (long)timeMs + " ms");
}
}
}
private void InputDataTimer_Tick(object sender, EventArgs e)
{
ROV rov = robotThread.Robot;
}
//解包
public static void DataHandler(List <byte> Buffer, ref ROV rov)
{
byte[] RxBuffer = Buffer.ToArray();
//功能选择，机械臂开合，充放气
//这里我还没写好
if (RxBuffer[0] == Startbit_Motion)
{
rov.engine.engine1_speed = BitConverter.ToInt16(RxBuffer, 1);
rov.engine.engine2_speed = BitConverter.ToInt16(RxBuffer, 3);
rov.engine.engine3_speed = BitConverter.ToInt16(RxBuffer, 5);
rov.engine.engine4_speed = BitConverter.ToInt16(RxBuffer, 7);
rov.engine.engine5_speed = BitConverter.ToInt16(RxBuffer, 9);
rov.engine.engine6_speed = BitConverter.ToInt16(RxBuffer, 11);
rov.sensor.angle_pitch = BitConverter.ToSingle(RxBuffer, 13);
rov.sensor.angle_roll = BitConverter.ToSingle(RxBuffer, 17);
rov.sensor.gyro_x = BitConverter.ToSingle(RxBuffer, 21);
rov.sensor.gyro_y = BitConverter.ToSingle(RxBuffer, 25);
rov.sensor.gyro_z = BitConverter.ToSingle(RxBuffer, 29);
rov.sensor.deapth = BitConverter.ToSingle(RxBuffer, 33);
rov.mode_value = BitConverter.ToUInt16(RxBuffer, 37);
rov.servo.servo1 = BitConverter.ToUInt16(RxBuffer, 39);
rov.servo.servo2 = BitConverter.ToUInt16(RxBuffer, 41);
rov.servo.servo3 = BitConverter.ToUInt16(RxBuffer, 43);
rov.servo.servo4 = BitConverter.ToUInt16(RxBuffer, 45);
}
if (RxBuffer[0] == Startbit_PIDData)
{
rov.pidData_pitch.P = BitConverter.ToSingle(RxBuffer, 1);
rov.pidData_pitch.I = BitConverter.ToSingle(RxBuffer, 5);
rov.pidData_pitch.D = BitConverter.ToSingle(RxBuffer, 9);
rov.pidData_pitch_w.P = BitConverter.ToSingle(RxBuffer, 13);
rov.pidData_pitch_w.I = BitConverter.ToSingle(RxBuffer, 17);
rov.pidData_pitch_w.D = BitConverter.ToSingle(RxBuffer, 21);
rov.pidData_depth.P = BitConverter.ToSingle(RxBuffer, 25);
rov.pidData_depth.I = BitConverter.ToSingle(RxBuffer, 29);
rov.pidData_depth.D = BitConverter.ToSingle(RxBuffer, 33);
rov.pidData_yaw_w.P = BitConverter.ToSingle(RxBuffer, 37);
rov.pidData_yaw_w.I = BitConverter.ToSingle(RxBuffer, 41);
rov.pidData_yaw_w.D = BitConverter.ToSingle(RxBuffer, 45);
if (!(Form_PID1 == null || Form_PID1.IsDisposed))
{
Form_PID1.PID_ParaTextChange();//更新当前PID值
}
}
if (RxBuffer[0] == Startbit_Motion_Control)
{
rov.setpara.depth_set = BitConverter.ToSingle(RxBuffer, 1);
rov.setpara.pitch_set = BitConverter.ToSingle(RxBuffer, 5);
rov.setpara.yaw_w_set = BitConverter.ToSingle(RxBuffer, 9);
if (!(Form_PID1 == null || Form_PID1.IsDisposed))
{
Form_PID1.PID_ParaTextChange();//更新当前PID值
}
}
}
//装包
public static byte[] DataToSendProcessor(byte startbit, ROV rov)
{
byte endbit = 0x00;
List <byte> TXBuffer = new List <byte>
{
startbit
};
if (startbit == Startbit_Engine_Control)
{
TXBuffer.AddRange(BitConverter.GetBytes(rov.engine.engine1_speed));
TXBuffer.AddRange(BitConverter.GetBytes(rov.engine.engine2_speed));
TXBuffer.AddRange(BitConverter.GetBytes(rov.engine.engine3_speed));
TXBuffer.AddRange(BitConverter.GetBytes(rov.engine.engine4_speed));
TXBuffer.AddRange(BitConverter.GetBytes(rov.engine.engine5_speed));
TXBuffer.AddRange(BitConverter.GetBytes(rov.engine.engine6_speed));
}
if (startbit == Startbit_PIDData)
{
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_pitch.P));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_pitch.I));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_pitch.D));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_pitch_w.P));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_pitch_w.I));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_pitch_w.D));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_depth.P));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_depth.I));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_depth.D));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_yaw_w.P));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_yaw_w.I));
TXBuffer.AddRange(BitConverter.GetBytes(rov.pidData_yaw_w.D));
}
if (startbit == Startbit_Motion_Control)
{
TXBuffer.AddRange(BitConverter.GetBytes(rov.setpara.depth_set));
TXBuffer.AddRange(BitConverter.GetBytes(rov.setpara.pitch_set));
TXBuffer.AddRange(BitConverter.GetBytes(rov.setpara.yaw_w_set));
}
if (startbit == Startbit_Mode)
{
TXBuffer.AddRange(BitConverter.GetBytes(rov.mode_value));
}
//键盘部分
if (startbit == Startbit_Key)
{
//TXBuffer.Add(Convert.ToByte((handle.Left_x + 1) / 2 * 255));
//TXBuffer.Add(Convert.ToByte((handle.Left_y + 1) / 2 * 255));
//TXBuffer.Add(Convert.ToByte((handle.Right_x + 1) / 2 * 255));
//TXBuffer.Add(Convert.ToByte((handle.Right_y + 1) / 2 * 255));
}
if (startbit == Startbit_Camera)
{
TXBuffer.Add(Convert.ToByte(rov.Camera_angle));
}
if (startbit == Startbit_Servo)
{
TXBuffer.AddRange(BitConverter.GetBytes(rov.servo.servo1));
TXBuffer.AddRange(BitConverter.GetBytes(rov.servo.servo2));
TXBuffer.AddRange(BitConverter.GetBytes(rov.servo.servo3));
TXBuffer.AddRange(BitConverter.GetBytes(rov.servo.servo4));
}
for (int i = 0; i < TXBuffer.Count; i++)
{
endbit += TXBuffer[i];
}
TXBuffer.Add(endbit);
byte[] TXData = TXBuffer.ToArray();
return(TXData);
}
public ActionResult Create(FormCollection collection)
{
var rovname = "";
long rovid;
ViewBag.successMessage = "";
ViewBag.errorMessage = "";
try
{
var action = Request.Form["action"].ToString();
if (action == "create")
{
rovname = Request.Form["rovname"].ToString();
var obj_rov = (
from p in db.ROVs
where (p.name == rovname && p.active == true)
select p
).FirstOrDefault();
if (obj_rov != null)
{
ViewBag.successMessage = "";
ViewBag.errorMessage = "Reason of Visit already exists";
}
if (obj_rov == null)
{
ROV dbrov = new ROV();
dbrov.name = rovname;
dbrov.cd = DateTime.Now;
dbrov.cb = SessionHandler.UserId;
dbrov.active = true;
db.ROVs.Add(dbrov);
db.SaveChanges();
ViewBag.successMessage = "Record has been saved successfully";
ViewBag.errorMessage = "";
}
}
if (action == "edit")
{
rovid = Convert.ToInt32(Request.Form["id"].ToString());
rovname = Request.Form["rovname"].ToString();
ROV dbrov = new ROV();
dbrov = db.ROVs.Where(r => r.rovID == rovid).FirstOrDefault();
dbrov.name = rovname;
dbrov.md = DateTime.Now;
dbrov.mb = SessionHandler.UserId;
db.Entry(dbrov).State = EntityState.Modified;
db.SaveChanges();
ViewBag.successMessage = "Record has been saved successfully";
ViewBag.errorMessage = "";
}
if (action == "delete")
{
rovid = Convert.ToInt32(Request.Form["id"].ToString());
ROV dbrov = new ROV();
dbrov = db.ROVs.Where(r => r.rovID == rovid).FirstOrDefault();
dbrov.active = false;
db.Entry(dbrov).State = EntityState.Modified;
db.SaveChanges();
ViewBag.successMessage = "Record has been deleted successfully";
ViewBag.errorMessage = "";
}
var rov = db.ROVs.Where(r => r.active == true).ToList();
return(View(rov));
}
catch (Exception ex)
{
ViewBag.errorMessage = "Error occurred while processing your request.";
var _rov = db.ROVs.Where(r => r.active == true).ToList();
return(View(_rov));
}
}
protected override bool Loop()
{
ROV robot = robotThread.Robot;
if ((robot != null) && (Enabled))
{
//Grab all inputs at once
Twist input = inputThread.Input;
bool gripperLOpen = inputThread.GripperLOpen;
bool gripperROpen = inputThread.GripperROpen;
bool netOpen = inputThread.NetOpen;
//Move servos for direction control
robot.PwmA1.DutyCycle = input.Linear.X;
robot.PwmD1.DutyCycle = input.Linear.X;
//Move gripper L
GripperPosition gripperL = GripperL;
if (gripperL != null)
{
if (gripperLOpen)
{
gripperL.Open(robot.LeftGripperServo);
}
else
{
gripperL.Close(robot.LeftGripperServo);
}
}
//Move gripper R
GripperPosition gripperR = GripperR;
if (gripperR != null)
{
if (gripperROpen)
{
gripperR.Open(robot.RightGripperServo);
}
else
{
gripperR.Close(robot.RightGripperServo);
}
}
//Move Net
GripperPosition net = Settings.NetGripper;
if (net != null)
{
if (netOpen)
{
net.Open(robot.NetServo);
}
else
{
net.Close(robot.NetServo);
}
}
}
return(Sleep(10));
}
