// Update is called once per frame
void Update()
{
//根据输入的角度进行计算
axis1_rot = Quaternion.AngleAxis(axis1_angle, Vector3.up);
axis2_rot = Quaternion.AngleAxis(axis2_angle, Vector3.right);
axis3_rot = Quaternion.AngleAxis(axis3_angle, Vector3.right);
axis1_tf.localRotation = axis1_rot;
axis2_tf.localRotation = axis2_rot;
axis3_tf.localRotation = axis3_rot;
//齐次坐标变换矩阵
T01.SetTRS(new Vector3(0f, 1f, 0f),
Quaternion.AngleAxis(axis1_angle, Vector3.up),
new Vector3(1f, 1f, 1f));
T12.SetTRS(new Vector3(0f, 1f, 0f),
Quaternion.AngleAxis(axis2_angle, Vector3.right),
new Vector3(1f, 1f, 1f));
T23.SetTRS(new Vector3(0f, 3f, 0f),
Quaternion.AngleAxis(axis3_angle, Vector3.right),
new Vector3(1f, 1f, 1f));
//正运动学计算 手臂的位置
hand_pos_world = T01 * T12 * T23 * hand_pos_local;
//UI上表示
transform_label.text = "Transform X:" + hand_transform.position.x.ToString("F2") +
" Y:" + hand_transform.position.y.ToString("F2") +
" Z:" + hand_transform.position.z.ToString("F2");
forward_K_label.text = "Forward K X:" + hand_pos_world.x.ToString("F2") +
" Y:" + hand_pos_world.y.ToString("F2") +
" Z:" + hand_pos_world.z.ToString("F2");
}
private void StageRev(int page)
{
WagoIO.Instance.SetListTC(lst, page);
T1.StageRev("АВАРИЯ^НОСИТЕЛЯ", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T2.StageRev("", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T3.StageRev("Р по^ПАДАЕТ", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T4.StageRev("СРАБОТАЛ^ТД", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T5.StageRev("", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T6.StageRev("ОТКАЗ^БВС", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T7.StageRev("Pса^ПАДАЕТ", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T8.StageRev("РАЗГЕРМ^СА", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T9.StageRev("РАЗГЕРМ^КЖО", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T10.StageRev("Р400^мм рт ст", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T11.StageRev("СОСТАВ^ВОЗДУХА", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T12.StageRev("РАЗГЕРМ^КНР", new SolidColorBrush(Colors.Red), new SolidColorBrush(Colors.Silver));
T13.StageRev("PАЗГЕРМ^ПОДГОТ", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T14.StageRev("РАЗДЕЛЕН^по ГЦ", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T15.StageRev("ОТКРЫТ^КСД БО", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T16.StageRev("АВАРИЯ^ЦВМ", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T17.StageRev("ПРОГР^РАЗДЕЛЕН", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T18.StageRev("ТД^ПОДКЛ", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T19.StageRev("ПРИЗНАК^СПУСК", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T20.StageRev("СГ^РАЗАРР", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T21.StageRev("РАБОТА^ДПО", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T22.StageRev("СПУСК^ПО ГЦ", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T23.StageRev("ВЫБОР^ДПО", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T24.StageRev("РАБОТА^СКД", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
Block1 = "TC-1";
Block2 = "TC-2";
Block3 = "TC-3";
Block4 = "TC-4";
if (page == 2)
{
T1.StageRev("БС", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T2.StageRev("БСР", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T3.StageRev("закрутка^СА", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T4.StageRev("ВЫЗОВ НА^СВЯЗЬ", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T5.StageRev("разреш^РРСТ", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T6.StageRev("U мало", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T7.StageRev("ввод^ЗСП", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T8.StageRev("отказ^очист СА", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T9.StageRev("ПОСАДКА", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T10.StageRev("РБ^подкл", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T11.StageRev("отказ^очист БО", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T12.StageRev("останов^КС-020", new SolidColorBrush(Colors.Yellow), new SolidColorBrush(Colors.Silver));
T13.StageRev("БВC^готов", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T14.StageRev("ОСК", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T15.StageRev("ГСО", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T16.StageRev("2 СЕКЦИЯ^НАДДУВА", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T17.StageRev("2 СЕКЦИЯ^КДУ", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T18.StageRev("СА-БО^люк откр", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T19.StageRev("РО АК", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T20.StageRev("ввод^ОСП", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T21.StageRev("подача^O2", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T22.StageRev("РУД", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T23.StageRev("УКВ^передача", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
T24.StageRev("МЕХАН^СОЕДИН", new SolidColorBrush(Colors.Green), new SolidColorBrush(Colors.Silver));
Block1 = "TC-5";
Block2 = "TC-6";
Block3 = "TC-7";
Block4 = "TC-8";
}
}
static bool Do23(T23 t) {
Nullable<T23> n = new Nullable<T23>(t);
return Ensure(n.HasValue && n.Value == t);
}
