/// <summary>
/// Called after we draw the last servo in a group
/// </summary>
/// <param name="groupID">What Group was it</param>
internal static void EndDrawGroup(int groupID)
{
if (Disabled)
{
return; //If the Drag and Drop is Disabled then just go back
}
//Only do this if there is a group that contains Servos
if (Groups.Count < 1 || Servos.Count < 1 || Servos.All(x => x.GroupID != groupID))
{
return;
}
try
{
//Set the height of the group to contain all the servos
var newRect = new Rect(Groups[groupID].GroupRect)
{
height = Servos.Last(x => x.GroupID == groupID).ServoRect.y +
Servos.Last(x => x.GroupID == groupID).ServoRect.height -
Groups[groupID].GroupRect.y
};
Groups[groupID].GroupRect = newRect;
}
catch (Exception ex) // Intentional Pokemon
{
Logger.Log(ex.Message, Logger.Level.Fatal);
}
}
public RemoteAim(String vcd)
{
InitializeComponent();
VideoCaptureDevice videoSource = new VideoCaptureDevice(vcd, new Size(320, 240), false);
OpenVideoSource(videoSource);
redDot.Location = new Point(gridBox.Width / 2 + gridBox.Left, gridBox.Height / 2 + gridBox.Top);
servos = new Servos();
XTextBox.Text = servos.GetMathPosition().X.ToString();
YTextBox.Text = servos.GetMathPosition().Y.ToString();
redDot.Location = servos.GetPorportionalMathPosition(gridBox.Bounds);
redDot.Location = new Point(redDot.Location.X - REDDOT_OFFSET_X, redDot.Location.Y - REDDOT_OFFSET_Y);
label1.Text = "(-" + servos.CenterServosPosition.X + "," + servos.CenterServosPosition.Y + ")";
PosIncTextBox.Text = positionIncrement.ToString();
textBoxXServo.Text = servos.ShootingRange.Width.ToString();
textBoxYServo.Text = servos.ShootingRange.Height.ToString();
textBoxXCoord.Text = servos.ServosPosition.X.ToString();
textBoxYCoord.Text = servos.ServosPosition.Y.ToString();
Packet packet = new Packet(servos.CenterServosPosition);
packet.setFireOff();
this.sendData(packet);
}
static void Main(string[] args)
{
try
{
string portName = SerialPortHelper.FindProlificPortName();
using (SSC32Board board = new SSC32Board(portName))
{
Servos servos = new Servos();
servos.ConfigureFromFile("ConfigSSC32.cfg");
Servo baseServo = servos.BaseServo;
System.Console.WriteLine(baseServo);
baseServo.Move(-30, 100, board);
baseServo.Move(30, 100, board);
}
}
catch (Exception ex)
{
System.Console.WriteLine(ex.ToString());
}
finally
{
System.Console.WriteLine("Hit <Enter> to end.");
System.Console.ReadLine();
}
}
static void Main(string[] args)
{
try
{
string portName = SerialPortHelper.FindProlificPortName();
using (SSC32Board board = new SSC32Board(portName))
{
Servos servos = new Servos();
servos.ConfigureFromFile("ConfigSSC32.cfg");
Servo baseServo = servos.BaseServo;
System.Console.WriteLine(baseServo);
baseServo.Move(-30, 100, board);
baseServo.Move(30, 100, board);
}
}
catch (Exception ex)
{
System.Console.WriteLine(ex.ToString());
}
finally
{
System.Console.WriteLine("Hit <Enter> to end.");
System.Console.ReadLine();
}
}
public FPSAim(String vcd)
{
InitializeComponent();
VideoCaptureDevice videoSource = new VideoCaptureDevice(vcd, new Size(320, 240), false);
OpenVideoSource(videoSource);
redDot.Location = new Point(gridBox.Width / 2 + gridBox.Left, gridBox.Height / 2 + gridBox.Top);
servos = new Servos();
redDot.Location = servos.GetPorportionalMathPosition(gridBox.Bounds);
redDot.Location = new Point(redDot.Location.X - REDDOT_OFFSET_X, redDot.Location.Y - REDDOT_OFFSET_Y);
label1.Text = "(-" + servos.CenterServosPosition.X + "," + servos.CenterServosPosition.Y + ")";
textBoxXServo.Text = servos.ShootingRange.Width.ToString();
textBoxYServo.Text = servos.ShootingRange.Height.ToString();
textBoxXCoord.Text = servos.ServosPosition.X.ToString();
textBoxYCoord.Text = servos.ServosPosition.Y.ToString();
Packet packet = new Packet(servos.CenterServosPosition);
packet.setFireOff();
this.sendData(packet);
Cursor.Hide();
cursorhidden = true;
CoordinateTimer.Start();
//Point camwindowcenter = new Point(gridBox.PointToScreen(new Point(0, 0)).X , gridBox.PointToScreen(new Point(0, 0)).Y );
//Cursor.Position = camwindowcenter;
currentX = Cursor.Position.X;
currentY = Cursor.Position.Y;
lastX = 0;
lastY = 0;
}
public RobotDriver()
{
m_Servos = new Servos();
RestoreRobotCalibration();
RestoreCameraCalibration();
MoveRawCommand.DefaultSpeed = 300;
}
private void SetTextBoxValues()
{
Servos mainServos = mainForm.MainServos;
XCenterTextBox.Text = mainServos.CenterServosPosition.X.ToString();
YCenterTextBox.Text = mainServos.CenterServosPosition.Y.ToString();
XRangeTextBox.Text = mainServos.ShootingRange.Width.ToString();
YRangeTextBox.Text = mainServos.ShootingRange.Height.ToString();
}
public void MovePrevPreset()
{
if (Servos.Any())
{
foreach (var servo in Servos)
{
servo.Preset.MovePrev();
}
}
}
public void MoveLeft()
{
if (Servos.Any())
{
foreach (var servo in Servos)
{
servo.Mechanism.MoveLeft();
}
}
}
public MainFormModel()
{
m_Capture = new Capture();
m_Capture.FlipHorizontal = true;
m_Capture.FlipVertical = true;
m_Servos = new Servos();
RestoreRobotCalibration();
RestoreCameraCalibration();
}
public void MoveCenter()
{
if (Servos.Any())
{
foreach (var servo in Servos)
{
servo.Motor.MoveCenter();
}
}
}
public void Stop()
{
MovingNegative = false;
MovingPositive = false;
if (Servos.Any())
{
foreach (var servo in Servos)
{
servo.Motor.Stop();
}
}
}
public async Task <float> GetServoAngleAsync(Servos servo)
{
if (servo == Servos.Hand)
{
throw new ArgumentException("Only Bottom, Left, Right joint angle can be read");
}
var res = await Transact(
string.Format(Protocol.GetServoAngleFormat, (int)servo)
);
return(res.ToFloat());
}
public MainForm()
{
InitializeComponent();
m_Servos = new Servos();
foreach (Control control in this.Controls)
{
ServoControl servoControl = control as ServoControl;
if (servoControl != null)
{
m_ServoControls.Add(servoControl);
}
}
}
public MainForm()
{
InitializeComponent();
m_Servos = new Servos();
foreach (Control control in this.Controls)
{
ServoControl servoControl = control as ServoControl;
if (servoControl != null)
{
m_ServoControls.Add(servoControl);
}
}
}
/// <summary>
/// Draw the Group Handle and do any maths
/// </summary>
/// <param name="name">Text name of the Servo</param>
/// <param name="groupID">Index of the Group</param>
/// <param name="servoID">Index of the Servo</param>
internal static void DrawServoHandle(string name, int groupID, int servoID)
{
if (Disabled)
{
return; //If the Drag and Drop is Disabled then just go back
}
//Draw the drag handle
GUILayout.Label(ImgDragHandle);
if (Event.current.type == EventType.Repaint)
{
//If its the repaint event then use GUILayoutUtility to get the location of the handle
//And build the structure so we know where on the screen it is
Servos.Add(name, groupID, servoID, GUILayoutUtility.GetLastRect(), WindowRect.width);
}
}
public TransmitPosition()
{
showErrorFlag = false;
InitializeComponent();
//usbHub = new usb_interface();
redDot.Location = new Point(gridBox.Width / 2 + gridBox.Left, gridBox.Height / 2 + gridBox.Top);
servos = new Servos(1630, 1477);
XTextBox.Text = servos.GetMathPosition().X.ToString();
YTextBox.Text = servos.GetMathPosition().Y.ToString();
redDot.Location = servos.GetPorportionalMathPosition(gridBox.Bounds);
redDot.Location = new Point(redDot.Location.X - REDDOT_OFFSET_X, redDot.Location.Y - REDDOT_OFFSET_Y);
label1.Text = "(-" + servos.ShootingRange.Width / 2 + ",-" + servos.ShootingRange.Height / 2 + ")";
PosIncTextBox.Text = positionIncrement.ToString();
//Packet packet = new Packet(servos.CenterServosPosition);
//packet.setFireOff();
//this.sendData(packet);
}
private void Freshen()
{
if (Servos == null)
{
return;
}
if (UseElectricCharge)
{
float chargeRequired = Servos.Where(s => s.Mechanism.IsFreeMoving == false).Sum(s => s.ElectricChargeRequired);
foreach (var servo in Servos)
{
servo.Group.ElectricChargeRequired = chargeRequired;
}
totalElectricChargeRequirement = chargeRequired;
}
stale = false;
}
public MainWindowViewModel()
{
m_Servos = new Servos();
RestoreCalibration();
this.X = Settings.Default.X;
this.Y = Settings.Default.Y;
this.Z = Settings.Default.Z;
this.ZIsBasedOnEndeffectorTip = Settings.Default.ZIsBasedOnTip;
if (this.X == 0 && this.Y == 0 && this.Z == 0)
{
this.X = RobotArm.ForearmLength;
this.Y = 0;
this.Z = RobotArm.ShoulderHeight + RobotArm.UpperArmLength;
this.ZIsBasedOnEndeffectorTip = false;
}
MoveRawCommand.DefaultSpeed = 500;
}
public bool SetServoSpeed(Servos servo, int speed)
{
ArduinoCommand cmd = new ArduinoCommand()
{
Command = Command.SetServoSpeed,
NumberOfReturnedBytes = 0
};
cmd.AddParameter((int)servo);
cmd.AddParameter(speed);
try
{
byte[] toWrite = cmd.GetCommandBytes();
stream.Write(toWrite, 0, toWrite.Length);
return(true);
}
catch (Exception)
{
return(false);
}
}
/*!
* This function (that executes in a separate thread) is an
* infinite loop that receives all lines from the serial port
* and parses and executes them.
*/
private object ReceiveThreadedData(object state)
{
//_serialPort.ReadTimeout = 1000;
bool recognised_frame = true;
string line = string.Empty;
while (file_to_replay != null && file_to_replay.BaseStream != null && !file_to_replay.EndOfStream)
{
try
{
while (!Play)
{
Thread.Sleep(100);
}
KeyValuePair <TimeSpan, string> kvp = ReadReplayLine();
if (DoubleSpeed)
{
Thread.Sleep((int)(kvp.Key.TotalMilliseconds / 2.0));
}
else if (QuadSpeed)
{
Thread.Sleep((int)(kvp.Key.TotalMilliseconds / 4.0));
}
else
{
Thread.Sleep(kvp.Key);
}
//Console.WriteLine(kvp.Key.ToString() + " - " + kvp.Value);
line = kvp.Value;
//line = _serialPort.ReadLine();
if (line.StartsWith("$")) // line with checksum
{
string[] frame = line.Substring(1, line.Length - 1).Split('*');
//line = frame[0];
if (calculateChecksum(frame[0]) == Int32.Parse(frame[1], System.Globalization.NumberStyles.HexNumber))
{
line = frame[0];
}
else
{
throw new Exception("Checksum error");
}
}
if (logfile != null)
{
logfile.WriteLine("[" + DateTime.Now.ToString("MM/dd/yyyy HH:mm:ss") + "] " + line);
}
lock (this)
{
bytes_read += line.Length + 1;
}
line = line.Replace("\r", "");
string[] lines = line.Split(';');
//Console.WriteLine(line + "\n\r");
// TR: Gyro & Acc raw
recognised_frame = true;
if (lines[0].EndsWith("TR") && lines.Length >= 6)
{
double acc_x_raw = double.Parse(lines[1]);
double acc_y_raw = double.Parse(lines[2]);
double acc_z_raw = double.Parse(lines[3]);
double gyro_x = double.Parse(lines[4]);
double gyro_y = double.Parse(lines[5]);
double gyro_z = double.Parse(lines[6]);
GyroAccRaw ga = new GyroAccRaw(acc_x_raw, acc_y_raw, acc_z_raw, gyro_x, gyro_y, gyro_z);
if (GyroAccRawCommunicationReceived != null)
{
GyroAccRawCommunicationReceived(ga);
}
}
// TP: Processed gyro & acc
else if (lines[0].EndsWith("TP") && lines.Length >= 6)
{
double acc_x = double.Parse(lines[1]) / 1000.0;
double acc_y = double.Parse(lines[2]) / 1000.0;
double acc_z = double.Parse(lines[3]) / 1000.0;
double gyro_x = double.Parse(lines[4]) / 1000.0 * 180.0 / 3.14;
double gyro_y = double.Parse(lines[5]) / 1000.0 * 180.0 / 3.14;
double gyro_z = double.Parse(lines[6]) / 1000.0 * 180.0 / 3.14;
GyroAccProcessed ga = new GyroAccProcessed(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z);
if (GyroAccProcCommunicationReceived != null)
{
GyroAccProcCommunicationReceived(ga);
}
}
// TH: Pressure & Temp
else if (lines[0].EndsWith("TH") && lines.Length >= 2)
{
float pressure = float.Parse(lines[1]);
float temp = float.Parse(lines[2]);
PressureTemp pt = new PressureTemp(temp, pressure);
if (PressureTempCommunicationReceived != null)
{
PressureTempCommunicationReceived(pt);
}
}
// CA: All configuration
else if (lines[0].EndsWith("CA") && lines.Length >= 2)
{
AllConfig ac = new AllConfig();
ac.acc_x_neutral = int.Parse(lines[1]);
ac.acc_y_neutral = int.Parse(lines[2]);
ac.acc_z_neutral = int.Parse(lines[3]);
ac.gyro_x_neutral = int.Parse(lines[4]);
ac.gyro_y_neutral = int.Parse(lines[5]);
ac.gyro_z_neutral = int.Parse(lines[6]);
ac.telemetry_basicgps = int.Parse(lines[7]);
ac.telemetry_ppm = int.Parse(lines[8]);
ac.telemetry_gyroaccraw = int.Parse(lines[9]);
ac.telemetry_gyroaccproc = int.Parse(lines[10]);
ac.telemetry_pressuretemp = int.Parse(lines[11]);
ac.telemetry_attitude = int.Parse(lines[12]);
ac.gps_initial_baudrate = int.Parse(lines[13]) * 10;
ac.gps_operational_baudrate = int.Parse(lines[14]) * 10;
ac.channel_ap = int.Parse(lines[15]) + 1;
ac.channel_motor = int.Parse(lines[16]) + 1;
ac.channel_pitch = int.Parse(lines[17]) + 1;
ac.channel_roll = int.Parse(lines[18]) + 1;
ac.channel_yaw = int.Parse(lines[19]) + 1;
ac.pid_pitch2elevator_p = double.Parse(lines[20], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_pitch2elevator_d = double.Parse(lines[21], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_pitch2elevator_i = double.Parse(lines[22], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_pitch2elevator_imin = double.Parse(lines[23], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_pitch2elevator_imax = double.Parse(lines[24], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_pitch2elevator_dmin = double.Parse(lines[25], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_roll2aileron_p = double.Parse(lines[26], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_roll2aileron_d = double.Parse(lines[27], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_roll2aileron_i = double.Parse(lines[28], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_roll2aileron_imin = double.Parse(lines[29], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_roll2aileron_imax = double.Parse(lines[30], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_roll2aileron_dmin = double.Parse(lines[31], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_heading2roll_p = double.Parse(lines[32], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_heading2roll_d = double.Parse(lines[33], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_heading2roll_i = double.Parse(lines[34], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_heading2roll_imin = double.Parse(lines[35], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_heading2roll_imax = double.Parse(lines[36], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_heading2roll_dmin = double.Parse(lines[37], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_altitude2pitch_p = double.Parse(lines[38], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_altitude2pitch_d = double.Parse(lines[39], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_altitude2pitch_i = double.Parse(lines[40], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_altitude2pitch_imin = double.Parse(lines[41], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_altitude2pitch_imax = double.Parse(lines[42], System.Globalization.CultureInfo.InvariantCulture);
ac.pid_altitude2pitch_dmin = double.Parse(lines[43], System.Globalization.CultureInfo.InvariantCulture);
int r = int.Parse(lines[44]);
byte r2 = (byte)r;
ac.servo_reverse[0] = (r & 1) != 0;
ac.servo_reverse[1] = (r & 2) != 0;
ac.servo_reverse[2] = (r & 4) != 0;
ac.servo_reverse[3] = (r & 8) != 0;
ac.servo_reverse[4] = (r & 16) != 0;
ac.servo_reverse[5] = (r & 32) != 0;
for (int i = 0; i < 6; i++)
{
ac.servo_min[i] = int.Parse(lines[45 + i * 3], System.Globalization.CultureInfo.InvariantCulture);
ac.servo_max[i] = int.Parse(lines[46 + i * 3], System.Globalization.CultureInfo.InvariantCulture);
ac.servo_neutral[i] = int.Parse(lines[47 + i * 3], System.Globalization.CultureInfo.InvariantCulture);
}
ac.rc_ppm = 1 - int.Parse(lines[63]);
ac.control_mixing = int.Parse(lines[64]);
ac.control_max_pitch = int.Parse(lines[65]);
ac.control_max_roll = int.Parse(lines[66]);
// for backwards compatibility
if (lines.Length > 67)
{
ac.control_waypoint_radius = int.Parse(lines[67]);
}
if (lines.Length > 68)
{
ac.control_cruising_speed = int.Parse(lines[68]);
}
if (lines.Length > 69)
{
ac.control_stabilization_with_altitude_hold = int.Parse(lines[69]) == 0 ? false : true;
}
if (lines.Length > 70)
{
ac.control_aileron_differential = int.Parse(lines[70]);
}
if (lines.Length > 71)
{
ac.telemetry_control = int.Parse(lines[71]);
}
if (lines.Length > 72)
{
ac.auto_throttle_enabled = int.Parse(lines[72]) == 1;
ac.auto_throttle_min_pct = int.Parse(lines[73]);
ac.auto_throttle_max_pct = int.Parse(lines[74]);
ac.auto_throttle_cruise_pct = int.Parse(lines[75]);
ac.auto_throttle_p_gain_10 = int.Parse(lines[76]);
}
if (lines.Length > 77)
{
ac.control_min_pitch = int.Parse(lines[77]);
}
if (lines.Length > 78)
{
ac.manual_trim = int.Parse(lines[78]) == 0 ? false : true;
Console.WriteLine("receive: " + lines[78]);
}
if (lines.Length > 79)
{
ac.control_altitude_mode = int.Parse(lines[79]);
}
else
{
Console.WriteLine("FOUT");
}
if (lines.Length > 80)
{
ac.gps_enable_waas = int.Parse(lines[80]);
}
else
{
Console.WriteLine("FOUT");
}
if (lines.Length > 81)
{
ac.osd_bitmask = int.Parse(lines[81]);
ac.osd_RssiMode = int.Parse(lines[82]);
ac.osd_voltage_low = ((double)int.Parse(lines[83])) / 50.0;
ac.osd_voltage_high = ((double)int.Parse(lines[84])) / 50.0;
}
if (lines.Length > 84)
{
ac.imu_rotated = int.Parse(lines[85]);
ac.neutral_pitch = int.Parse(lines[86]);
}
if (AllConfigCommunicationReceived != null)
{
AllConfigCommunicationReceived(ac);
}
}
// TT: RC transmitter
else if (lines[0].EndsWith("TT") && lines.Length >= 7)
{
RcInput rc = new RcInput(
new int[] {
int.Parse(lines[1]),
int.Parse(lines[2]),
int.Parse(lines[3]),
int.Parse(lines[4]),
int.Parse(lines[5]),
int.Parse(lines[6]),
int.Parse(lines[7])
});
if (RcInputCommunicationReceived != null)
{
RcInputCommunicationReceived(rc);
}
}
// TG: GPS basic
else if (lines[0].EndsWith("TG") && lines.Length >= 7)
{
//Console.WriteLine(line);
GpsBasic gb = new GpsBasic(
double.Parse(lines[2], System.Globalization.CultureInfo.InvariantCulture),
double.Parse(lines[3], System.Globalization.CultureInfo.InvariantCulture),
int.Parse(lines[7]),
double.Parse(lines[5]) / 100,
double.Parse(lines[4]) / 10,
int.Parse(lines[6]),
int.Parse(lines[1])
);
if (GpsBasicCommunicationReceived != null)
{
GpsBasicCommunicationReceived(gb);
}
}
// TA: Attitude
else if (lines[0].EndsWith("TA") && lines.Length >= 3)
{
Attitude att = new Attitude(
double.Parse(lines[1], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,
double.Parse(lines[2], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,
/*double.Parse(lines[3], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,
* double.Parse(lines[4], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,*/0, 0,
double.Parse(lines[3], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0
);
if (AttitudeCommunicationReceived != null)
{
AttitudeCommunicationReceived(att);
}
}
// DT: Datalog table
else if (lines[0].EndsWith("DT") && lines.Length >= 4)
{
DatalogTable dt = new DatalogTable(
int.Parse(lines[1]),
int.Parse(lines[3]),
int.Parse(lines[4]),
int.Parse(lines[2]),
0);
if (DatalogTableCommunicationReceived != null)
{
DatalogTableCommunicationReceived(dt);
}
}
// DH: Datalog header
else if (lines[0].EndsWith("DH") && lines.Length >= 4)
{
DatalogHeader = new string[lines.Length - 1];
for (int i = 1; i < lines.Length; i++)
{
DatalogHeader[i - 1] = lines[i];
}
}
// DD: Datalog data
else if (lines[0].EndsWith("DD") && lines.Length >= 4)
{
string[] logline = new string[DatalogHeader.Length];
for (int i = 0; i < logline.Length; i++)
{
logline[i] = lines[i + 1];
}
DatalogLine dl = new DatalogLine(
logline, DatalogHeader);
if (DatalogLineCommunicationReceived != null)
{
DatalogLineCommunicationReceived(dl);
}
}
// ND: Navigation data (Navigation instruction)
else if (lines[0].EndsWith("ND") && lines.Length >= 6)
{
Console.WriteLine(line);
lines[1] = lines[1].Replace("nan", "0");
lines[2] = lines[2].Replace("nan", "0");
lines[3] = lines[3].Replace("nan", "0");
lines[4] = lines[4].Replace("nan", "0");
lines[5] = lines[5].Replace("nan", "0");
lines[6] = lines[6].Replace("nan", "0");
NavigationInstruction ni =
new NavigationInstruction(
int.Parse(lines[1]),
(NavigationInstruction.navigation_command) int.Parse(lines[2]),
double.Parse(lines[3], CultureInfo.InvariantCulture),
double.Parse(lines[4], CultureInfo.InvariantCulture),
int.Parse(lines[5]),
int.Parse(lines[6]));
if (NavigationInstructionCommunicationReceived != null)
{
NavigationInstructionCommunicationReceived(ni);
}
}
// TS: Servos (simulation)
else if (lines[0].EndsWith("TS") && lines.Length >= 3)
{
Console.WriteLine(line);
Servos s =
new Servos(
int.Parse(lines[1]),
int.Parse(lines[2]),
int.Parse(lines[3]));
if (ServosCommunicationReceived != null)
{
ServosCommunicationReceived(s);
}
}
// Control
else if (lines[0].EndsWith("TC") && lines.Length >= 3)
{
ControlInfo ci =
new ControlInfo();
ci.FlightMode = (ControlInfo.FlightModes) int.Parse(lines[1]);
ci.CurrentNavigationLine = int.Parse(lines[2]);
ci.Altitude = int.Parse(lines[3]);
if (lines.Length >= 5)
{
ci.Batt1Voltage = double.Parse(lines[4]) / 10.0;
if (lines.Length >= 6)
{
ci.FlightTime = int.Parse(lines[5]);
ci.BlockTime = int.Parse(lines[6]);
ci.RcLink = int.Parse(lines[7]);
ci.Throttle = int.Parse(lines[8]);
}
if (lines.Length >= 10)
{
ci.TargetAltitude = int.Parse(lines[9]);
}
if (lines.Length >= 11)
{
ci.Batt2Voltage = double.Parse(lines[10]) / 10.0;
ci.Batt_mAh = double.Parse(lines[11]) * 10.0;
}
}
if (ControlInfoCommunicationReceived != null)
{
ControlInfoCommunicationReceived(ci);
}
}
else
{
recognised_frame = false;
Console.WriteLine(line);
if (NonParsedCommunicationReceived != null)
{
NonParsedCommunicationReceived(line);
}
}
}
catch (TimeoutException toe)
{
if (CommunicationAlive)
{
if (CommunicationLost != null && SecondsConnectionLost() >= 5.0)
{
CommunicationLost();
CommunicationAlive = false;
}
}
}
catch (IOException ioe)
{
// happens when thread is shut down
}
catch (Exception e)
{
;
}
try
{
if (recognised_frame)
{
//Console.WriteLine(line);
LastValidFrame = DateTime.Now;
FramesReceived++;
if (!CommunicationAlive)
{
CommunicationAlive = true;
if (CommunicationEstablished != null)
{
CommunicationEstablished();
}
}
}
if (CommunicationReceived != null)
{
CommunicationReceived(line);
}
}
catch (Exception e)
{
;
}
}
CommunicationAlive = false;
CommunicationLost();
return(null);
}
public Task <bool> CheckMotorAttachedAsync(Servos servo) => BoolTransact(string.Format(Protocol.CheckMotorAttachedFormat, (int)servo));
public Task DetachMotorAsync(Servos servo) => SetCommandTransact(string.Format(Protocol.DetachMotorFormat, (int)servo));
public Task MoveServoAngleAsync(Servos servo, float angle) => SetCommandTransact(string.Format(Protocol.MoveServoFormat, (int)servo, angle));
internal static void WindowEnd()
{
if (Disabled)
{
return; //If the Drag and Drop is Disabled then just go back
}
// Draw the Yellow insertion strip
Rect insertRect;
if (DraggingItem && ServoDragging != null && ServoOver != null)
{
//What is the insert position of the dragged servo
int insertIndex = ServoOver.ID + (ServoOverUpper ? 0 : 1);
if ((ServoDragging.GroupID != ServoOver.GroupID) ||
(ServoDragging.ID != insertIndex && (ServoDragging.ID + 1) != insertIndex))
{
//Only in here if the drop will cause the list to change
float rectResMoveY;
//is it dropping in the list or at the end
if (insertIndex < Servos.Where(x => x.GroupID == ServoOver.GroupID).ToList().Count)
{
rectResMoveY =
Servos.Where(x => x.GroupID == ServoOver.GroupID).ToList()[insertIndex].ServoRect.y;
}
else
{
rectResMoveY = Servos.Where(x => x.GroupID == ServoOver.GroupID).ToList().Last().ServoRect.y +
Servos.Where(x => x.GroupID == ServoOver.GroupID)
.ToList()
.Last()
.ServoRect.height;
}
//calculate and draw the graphic
insertRect = new Rect(12,
rectResMoveY + 26 - ScrollPosition.y,
WindowRect.width - 34, 9);
GUI.Box(insertRect, "", StyleDragInsert);
}
}
else if (DraggingItem && GroupDragging != null && GroupOver != null)
{
//What is the insert position of the dragged group
int insertIndex = GroupOver.ID + (GroupOverUpper ? 0 : 1);
if (GroupDragging.ID != insertIndex && (GroupDragging.ID + 1) != insertIndex)
{
//Only in here if the drop will cause the list to change
float rectResMoveY;
//is it dropping in the list or at the end
if (insertIndex < Groups.Count)
{
rectResMoveY = Groups[insertIndex].GroupRect.y;
}
else
{
rectResMoveY = Groups.Last().GroupRect.y + Groups.Last().GroupRect.height;
}
//calculate and draw the graphic
insertRect = new Rect(12,
rectResMoveY + 26 - ScrollPosition.y,
WindowRect.width - 34, 9);
GUI.Box(insertRect, "", StyleDragInsert);
}
}
else if (DraggingItem && ServoDragging != null && GroupOver != null &&
Servos.All(x => x.GroupID != GroupOver.ID))
{
//This is the case for an empty Group
//is it dropping in the list or at the end
float rectResMoveY = GroupOver.GroupRect.y + GroupOver.GroupRect.height;
//calculate and draw the graphic
insertRect = new Rect(12,
rectResMoveY + 26 - ScrollPosition.y,
WindowRect.width - 34, 9);
GUI.Box(insertRect, "", StyleDragInsert);
}
//What is the mouse over
if (MousePosition.y > 32 && MousePosition.y < WindowRect.height - 8)
{
//inside the scrollview
//check what group
GroupOver =
Groups.FirstOrDefault(x => x.GroupRect.Contains(MousePosition + ScrollPosition - new Vector2(8, 29)));
GroupIconOver =
Groups.FirstOrDefault(x => x.IconRect.Contains(MousePosition + ScrollPosition - new Vector2(8, 29)));
if (GroupOver != null)
{
GroupOverUpper = ((MousePosition + ScrollPosition - new Vector2(8, 29)).y - GroupOver.GroupRect.y) <
GroupOver.GroupRect.height / 2;
}
//or servo
ServoOver =
Servos.FirstOrDefault(x => x.ServoRect.Contains(MousePosition + ScrollPosition - new Vector2(8, 29)));
ServoIconOver =
Servos.FirstOrDefault(x => x.IconRect.Contains(MousePosition + ScrollPosition - new Vector2(8, 29)));
if (ServoOver != null)
{
ServoOverUpper = ((MousePosition + ScrollPosition - new Vector2(8, 29)).y - ServoOver.ServoRect.y) <
ServoOver.ServoRect.height / 2;
}
}
else
{
//Otherwise empty the variables
GroupOver = null;
ServoOver = null;
GroupIconOver = null;
ServoIconOver = null;
}
//MouseDown - left mouse button
if (Event.current.type == EventType.mouseDown &&
Event.current.button == 0)
{
if (GroupIconOver != null)
{
//If we click on the drag icon then start the drag
GroupDragging = GroupOver;
DraggingItem = true;
}
else if (ServoIconOver != null)
{
//If we click on the drag icon then start the drag
ServoDragging = ServoOver;
DraggingItem = true;
}
}
//did we release the mouse
if (Event.current.type == EventType.mouseUp &&
Event.current.button == 0)
{
if (GroupDragging != null && GroupOver != null)
{
//were we dragging a group
if (GroupDragging.ID != (GroupOver.ID + (GroupOverUpper ? 0 : 1)))
{
//And it will cause a reorder
Logger.Log(string.Format("Reordering:{0}-{1}", GroupDragging.ID,
(GroupOver.ID - (GroupOverUpper ? 1 : 0))));
//where are we inserting the dragged item
int insertAt = (GroupOver.ID - (GroupOverUpper ? 1 : 0));
if (GroupOver.ID < GroupDragging.ID)
{
insertAt += 1;
}
//move em around
ServoController.ControlGroup g = ServoController.Instance.ServoGroups[GroupDragging.ID];
ServoController.Instance.ServoGroups.RemoveAt(GroupDragging.ID);
ServoController.Instance.ServoGroups.Insert(insertAt, g);
}
}
else if (ServoDragging != null && ServoOver != null)
{
//were we dragging a servo
//where are we inserting the dragged item
int insertAt = (ServoOver.ID + (ServoOverUpper ? 0 : 1));
if (ServoOver.GroupID == ServoDragging.GroupID && ServoDragging.ID < ServoOver.ID)
{
insertAt -= 1;
}
Logger.Log(string.Format("Reordering:({0}-{1})->({2}-{3})", ServoDragging.GroupID, ServoDragging.ID,
ServoOver.GroupID, insertAt));
//move em around
IServo s = ServoController.Instance.ServoGroups[ServoDragging.GroupID].Servos[ServoDragging.ID];
ServoController.Instance.ServoGroups[ServoDragging.GroupID].Servos.RemoveAt(ServoDragging.ID);
ServoController.Instance.ServoGroups[ServoOver.GroupID].Servos.Insert(insertAt, s);
}
else if (ServoDragging != null && GroupOver != null && Servos.All(x => x.GroupID != GroupOver.ID))
{
//dragging a servo to an empty group
const int INSERT_AT = 0;
IServo s = ServoController.Instance.ServoGroups[ServoDragging.GroupID].Servos[ServoDragging.ID];
ServoController.Instance.ServoGroups[ServoDragging.GroupID].Servos.RemoveAt(ServoDragging.ID);
ServoController.Instance.ServoGroups[GroupOver.ID].Servos.Insert(INSERT_AT, s);
}
//reset the dragging stuff
DraggingItem = false;
GroupDragging = null;
ServoDragging = null;
}
//If we are dragging and in the bottom or top area then scrtoll the list
if (DraggingItem && RectScrollBottom.Contains(MousePosition))
{
ControlsGUI.SetEditorScrollYPosition(ScrollPosition.y + (Time.deltaTime * 40));
}
if (DraggingItem && RectScrollTop.Contains(MousePosition))
{
ControlsGUI.SetEditorScrollYPosition(ScrollPosition.y - (Time.deltaTime * 40));
}
}
