(start_flag) Start. Text = «Стоп»;Start. Text = «Запуск»;((gotovo == false) && (start_flag == true)) action();(start_flag)
{. Enabled = false;. Checked = true;
}groupBox1. Enabled =
true;
//// порграммное управление //// (start_flag && radioButton1. Checked) // одноразовое выполнение программы
{
if (listBox1. Items. Count > 0) // если есть строки
{
try
{st = listBox1. Items [listBox1. SelectedIndex].ToString(); // если нет выделенных строк
}
{. SetSelected (0, true); // выделяем 1 ую строку
}((listBox1. Items [listBox1. SelectedIndex].ToString().Length > 0) && gotovo) // если есть строка
{_pos = listBox1. Items [listBox1. SelectedIndex].ToString(); // считывание строки-команды(String. Equals (str_pos, str_pos_1) == false) gotovo = false; // если поступила новая команда, ставим флаг на выполнение команды
}(gotovo) // если команда выполнена
{
if (listBox1. Items. Count > (listBox1. SelectedIndex + 1)) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду
else start_flag = false; // если строк-команд нет, программа завершена
}
}start_flag = false; // если строк-команд нет, программа завершена
}
(start_flag && radioButton2. Checked) // выполнение в цикле
{(listBox1. Items. Count > 0) // если есть строки
{
{st = listBox1. Items [listBox1. SelectedIndex].ToString(); // если нет выделенных строк
}
{. SetSelected (0, true); // выделяем 1 ую строку
}((listBox1. Items [listBox1. SelectedIndex].ToString().Length > 0) && gotovo) // если есть строка
{_pos = listBox1. Items [listBox1. SelectedIndex].ToString(); // считывание строки-команды(String. Equals (str_pos, str_pos_1) == false) gotovo = false; // если поступила новая команда, ставим флаг на выполнение команды
}(gotovo) // если команда выполнена
{
if (listBox1. Items. Count > (listBox1. SelectedIndex + 1)) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду
else listBox1. SetSelected (0, true); // если строк-команд нет, переходим в начало
}
}start_flag = false; // если строк-команд нет, программа завершена
}
(start_flag && radioButton3. Checked) // пошаговое выполнение
{(listBox1. Items. Count > 0) // если есть строки
{
{st = listBox1. Items [listBox1. SelectedIndex].ToString(); // если нет выделенных строк
}
{. SetSelected (0, true); // выделяем 1 ую строку
}((listBox1. Items. Count > (listBox1. SelectedIndex + 1))) // если есть следующая строка
{
if ((listBox1. Items [listBox1. SelectedIndex + 1].ToString().Length > 0) && gotovo && (flag1==false)) // если есть строка
{(flag1 == false) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду
str_pos = listBox1. Items [listBox1. SelectedIndex].ToString(); // считывание строки-команды= false; // ставим флаг на выполнение команды= true; // ставим флаг, что начали выполнение программы
}(gotovo && flag1) // если команда выполнена
{= false;_flag = false; // программа завершена
}
}
{_flag = false; // если строк-команд нет, программа завершена
}
}start_flag = false; // если строк-команд нет, программа завершена
}
// вывод информации о состоянии программы
if (start_flag)
{. Text = «Выполнение программы»;(String. Equals (str1, «P»)) // если это команда позиции
{. Text = «Движение к позиции»;
label22. Text = «X=» + str_x +» Y=» + str_y +» Z=» + str_z;. Text = «Схват=» + str_s;
}(String. Equals (str1, «D»)) // если это команда задержки
{. Text = «Ожидание»;. Text = «time="+time1. ToString();. Text = «»;
}
}
{. Text = «»;. Text = «»;. Text = «»;23. Text = «»;
}
}
///// конец управления роботом //////
void korrektirovka() // корректировка положения углов
{f1 = false;
(q0 > 85)
{= 85;= true;
}(q0 < -85)
{= -85;= true;
}(q1 > 102)
{= 102;= true;
}(q1 < -42)
{= -42;= true;
}(q2 > 48)
{= 48;= true;
}(q2 < -90)
{= -90;= true;
}
((q3r * k) > 0)
{(q1 < 0) q1 = q1 + s0;(q2 < 0) q2 = q2 + s0;= true;
}
_zadacha();(f1)
{= Xm;= Ym;= Zm;= false;
}
}
void pryamaya_zadacha()
{
// -Прямая_задача- // = 57.295779513;q0r = q0 / k;q1r = q1 / k;q2r = q2 / k;r = - (q1r + q2r);
sin_q0 = Math. Sin(q0r);sin_q1 = Math. Sin(q1r);sin_q2 = Math. Sin(q2r);sin_q3 = Math. Sin(q3r);
cos_q0 = Math. Cos(q0r);cos_q1 = Math. Cos(q1r);cos_q2 = Math. Cos(q2r);cos_q3 = Math. Cos(q3r);
= L2 * (cos_q0 * cos_q1 * cos_q2 - cos_q0 * sin_q1 * sin_q2) + L3 * (cos_q3 * (cos_q0 * cos_q1 * cos_q2 - cos_q0 * sin_q1 * sin_q2) - sin_q3 * (cos_q0 * cos_q1 * sin_q2 + cos_q0 * cos_q2 * sin_q1)) + L1 * cos_q0 * sin_q1;= L2 * (cos_q1 * cos_q2 * sin_q0 - sin_q0 * sin_q1 * sin_q2) + L3 * (cos_q3 * (cos_q1 * cos_q2 * sin_q0 - sin_q0 * sin_q1 * sin_q2) - sin_q3 * (cos_q1 * sin_q0 * sin_q2 + cos_q2 * sin_q0 * sin_q1)) + L1 * sin_q0 * sin_q1;= L0 - L2 * (cos_q1 * sin_q2 + cos_q2 * sin_q1) - L3 * (cos_q3 * (cos_q1 * sin_q2 + cos_q2 * sin_q1) + sin_q3 * (cos_q1 * cos_q2 - sin_q1 * sin_q2)) + L1 * cos_q1;
= Math. Round(Xm);= Math. Round(Ym);= Math. Round(Zm);
}
void obratnaya_zadacha()
{
// -Обратная_задача- // xx = Math. Sqrt (X * X + Y * Y);x1 = xx - L3;z1 = Z - L0;B = Math. Sqrt (x1 * x1 + z1 * z1);
= Math. Asin (Y / xx);= 1.570796327 - Math. Acos((L1 * L1 + B * B - L2 * L2) / (2 * B * L2)) - Math. Asin (z1 / B);= 1.570796327 - Math. Acos((L1 * L1 + L2 * L2 - B * B) / (2 * L1 * L2));
= Math. Round (qq0 * k);= Math. Round (qq1 * k);= Math. Round (qq2 * k);
}
// обработчики нажатия кнопок управленияvoid Q0U_MouseDown (object sender, MouseEventArgs e)
{U = true;
}
void Q0U_MouseUp (object sender, MouseEventArgs e)
{U = false;
}
void Q0D_MouseDown (object sender, MouseEventArgs e)
{D = true;
}
void Q0D_MouseUp (object sender, MouseEventArgs e)
{D = false;
}
private void Q1U_MouseDown (object sender, MouseEventArgs e)
{U = true;
}
void Q1U_MouseUp (object sender, MouseEventArgs e)
{U = false;
}
void Q1D_MouseDown (object sender, MouseEventArgs e)
{D = true;
}
void Q1D_MouseUp (object sender, MouseEventArgs e)
{D = false;
}
void Q2U_MouseDown (object sender, MouseEventArgs e)
{U = true;
}
void Q2U_MouseUp (object sender, MouseEventArgs e)
{U = false;
}
private void Q2D_MouseDown (object sender, MouseEventArgs e)
{D = true;
}
void Q2D_MouseUp (object sender, MouseEventArgs e)
{D = false;
}
void Q3U_MouseDown (object sender, MouseEventArgs e)
{U = true;
}
void Q3U_MouseUp (object sender, MouseEventArgs e)
{U = false;
}
void Q3D_MouseDown (object sender, MouseEventArgs e)
{D = true;
}
void Q3D_MouseUp (object sender, MouseEventArgs e)
{D = false;
}
private void trackBar1_Scroll (object sender, EventArgs e)
{
// регулировка задержки таймера1
// отвечает за скорость скорость перемещения робота
int period1 = trackBar1. Value;= (101 - period1);. Interval = period1; // уровень задержки таймера. Text = (101 - period1).ToString()+ "%»;
}
void trackBar2_Scroll (object sender, EventArgs e)
{
// регулировка задержки таймера2
// отвечает за частоту отправки команд управления
int period2 = trackBar2. Value;period21 = 1000 / period2;= Math. Round(period21);. Interval = (int) period21; // уровень задержки таймера. Text = period2. ToString() + «команд/сек»;
}
void comboBox1_SelectedIndexChanged (object sender, EventArgs e)
{ // выбирает доступный COM порт, если он есть
if (comboBox1. SelectedItem. ToString()!= «») Port1 = new SerialPort (comboBox1. SelectedItem. ToString(), 9600);
}
void Poisk_Click (object sender, EventArgs e)
{
// поиск доступных COM портов[] myPort; // создаем массив строк
myPort = System.IO. Ports. SerialPort. GetPortNames(); // в массив помещаем доступные порты
comboBox1. Items. Clear(); // очистка comboBox. Items. AddRange(myPort); // теперь этот массив заносим в список(comboBox)
if (comboBox1. Items. Count!= 0) comboBox1. SelectedIndex = 0;
}
void timer2_Tick (object sender, EventArgs e)
{
// -Отправка_команды_на_Arduino- //
// преобразование углов для сервоприводов
double q0s = Math. Round (94 - q0);q1s = Math. Round (102 - q1);q2s = Math. Round (132 + q2);komanda = «»; // очистка строки
(System.IO. Ports. SerialPort. GetPortNames().Length == 0) comboBox1. Items. Clear(); // если нет портов очистка comboBox
(double. IsNaN(q0) || double. IsNaN(q1) || double. IsNaN(q2) || double. IsNaN(X) || double. IsNaN(Y) || double. IsNaN(Z))
{. ForeColor = Color. Red;
label9. Text = «Выход из рабочей зоны!»;
}
{= q0s. ToString() + «» + q1s. ToString() +» «+ q2s. ToString() +» «+ q3. ToString() +»!»; // создание строки значений углов поворотов звеньев
textBox1. Text = komanda;
(komanda!= stroka)
{(comboBox1. Items. Count!= 0) // если есть доступный COM порт
{. ForeColor = Color. LimeGreen;(flag) label9. Text = «COM порт подключен!»;
try
{
Port1 = new SerialPort (comboBox1. SelectedItem. ToString(), 9600);. Open(); // открываем порт. Write(komanda); // отправка строки значений углов
Port1. Close(); // закрываем порт
}(Exception)
{. ForeColor = Color. Red;(flag) label9. Text = «Ошибка подключения по COM порту!»;
}
}
{. ForeColor = Color. Red;(flag) label9. Text = «COM порт не подключен!»;
}
}
}= komanda;
// -Конец_отправки- //
}void button1_Click_1 (object sender, EventArgs e)
{position = «P» + X + «» + Y +» «+ Z +» " + q3;
// добавление в список позицию робота
if ((listBox1. Items. Count == 0) || (listBox1. SelectedIndex == (listBox1. Items. Count - 1)))
{. Items. Add(position);. SetSelected (listBox1. Items. Count - 1, true);
}
{. Items. Insert (listBox1. SelectedIndex + 1, position);
}
}void button2_Click_1 (object sender, EventArgs e)
{delay = «D» + textBox2. Text;
// добавление в список позицию робота
if ((listBox1. Items. Count == 0) || (listBox1. SelectedIndex == (listBox1. Items. Count - 1)))
{. Items. Add(delay);. SetSelected (listBox1. Items. Count - 1, true);
}
{. Items. Insert (listBox1. SelectedIndex + 1, delay);
}
}void button3_Click (object sender, EventArgs e)
{
// удаление выбранной строкиp = listBox1. SelectedIndex;(p >= 0) listBox1. Items. RemoveAt(p);(p - 1 >= 0) listBox1. SetSelected (p - 1, true);((p == 0) && (listBox1. Items. Count >= 1)) listBox1. SetSelected (p, true);
}void button4_Click (object sender, EventArgs e)
{. Items. Clear(); // очистить поле
}void button6_Click (object sender, EventArgs e)
{
// сохранение текста программы двмжения роботаmyStream;
SaveFileDialog saveFileDialog1 = new SaveFileDialog();. Filter = «txt files (*.txt)|*.txt»;. FilterIndex = 2;. RestoreDirectory = true;(saveFileDialog1. ShowDialog() == DialogResult.OK)
{((myStream = saveFileDialog1. OpenFile())!= null)
{sw = new StreamWriter(myStream);(int i = 0; i < listBox1. Items. Count; i++)
{. SelectedIndex = i;. WriteLine (listBox1. SelectedItem. ToString());
}. Close();. Close();
}
}
}void button7_Click (object sender, EventArgs e)
{
// загрузка текста программы движения роботаopenFileDial = new OpenFileDialog();line;(openFileDial. ShowDialog() == DialogResult.OK)
{. Items. Clear(); // очистить полеfileName = openFileDial. FileName;sss = File. Open (fileName, FileMode. Open, FileAccess. Read);(sss!= null)
{reader = new StreamReader(sss);((line = reader. ReadLine())!= null). Items. Add(line);. Close();
}
}
}void action()
{
// выполнение команды программы
if (String. Equals (str_pos, str_pos_1)==false) // если поступила новая команда
{_pos_1 = str_pos;= «»;_x = «»;_y = «»;_z = «»;_s = «»;_t = «»;i = 0;
// считывание команды(str_pos. Length > 0)
{((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))
{= str1 + str_pos[i];= i + 1;
}(String. Equals (str1, «P») == true) // если это команда позиции
{= i + 1;((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))
{_x = str_x + str_pos[i];= i + 1;
}= i + 1;((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))
{_y = str_y + str_pos[i];= i + 1;
}= i + 1;((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))
{_z = str_z + str_pos[i];= i + 1;
}= i + 1;((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))
{_s = str_s + str_pos[i];= i + 1;
}= int. Parse (str_x);= int. Parse (str_y);= int. Parse (str_z);= int. Parse (str_s);
///////////////////////
// -Обратная_задача- //
double xx = Math. Sqrt (xk * xk + yk * yk);
double x1 = xx - L3;z1 = zk - L0;B = Math. Sqrt (x1 * x1 + z1 * z1);= Math. Asin (yk / xx);= 1.570796327 - Math. Acos((L1 * L1 + B * B - L2 * L2) / (2 * B * L2)) - Math. Asin (z1 / B);= 1.570796327 - Math. Acos((L1 * L1 + L2 * L2 - B * B) / (2 * L1 * L2));= Math. Round (qq0 * k);= Math. Round (qq1 * k);= Math. Round (qq2 * k);
///////////////////////
}(String. Equals (str1, «D»)) // если это команда задержки
{= i + 1;((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))
{_t = str_t + str_pos[i];= i + 1;
}= int. Parse (str_t);
}
}
}
// выполнение команды(String. Equals (str1, «P»)) // если это команда позиции
{(q0 < qq0)
{= q0 + 1;(q0 > 85) q0 = 85;
}(q0 > qq0)
{= q0 - 1;(q0 < -85) q0 = -85;
}
//Q0T. Text = q0. ToString(); // вывод значения угла q0(q1 < qq1)
{= q1 + 1;(q1 > 102) q1 = 102;
}((q1 > qq1) && ((q3r * k) < 0))
{= q1 - 1;(q1 < -42) q1 = -42;
}
//Q1T. Text = q1. ToString(); // вывод значения угла q1
if (q2 < qq2)
{= q2 + 1;(q2 > 48) q2 = 48;
}((q2 > qq2) && ((q3r * k) < 0))
{= q2 - 1;(q2 < -90) q2 = -90;
}
//Q2T. Text = q2. ToString(); // вывод значения угла q2
if (q3 < sk)
{= q3 + 1;(q3 > 90) q3 = 90;
}(q3 > sk)
{= q3 - 1;(q3 < 0) q3 = 0;
}
//Q3T. Text = q3. ToString(); // вывод значения угла q3
if ((q3r * k) > 0)
{(q1 < 0) q1 = q1 + 1;(q2 < 0) q2 = q2 + 1;
}_zadacha();= Xm;= Ym;
Z = Zm;
// если все углы достигнуты, команда выполнена
if ((q0 == qq0) && (q1 == qq1) && (q2 == qq2) && (q3 == sk)) gotovo = true;
}(String. Equals (str1, «D»)) // если это команда задержки
{(time1 == 0) time1 = tk*10;_time = true;
}
}void Start_Click (object sender, EventArgs e)
{(start_flag == false) start_flag = true;start_flag = false;
}void timer3_Tick (object sender, EventArgs e)
{(f_time && start_flag)
{(time1 == 1)
{_time = false;= true;
}(time1 > 0) time1 = time1 - 1;
}
}void textBox2_KeyPress (object sender, KeyPressEventArgs e)
{
// ограничение наввод только цифр
if (Char. IsDigit (e. KeyChar) || (e. KeyChar == 8))
{
}
{. Handled = true;
}
}
}
}