Commit 1085ac7b authored by Alexey Hohlov's avatar Alexey Hohlov

Отладка ходьбы

parent 358c5b5b
...@@ -8,18 +8,11 @@ ...@@ -8,18 +8,11 @@
static QSerialPort tty_port; static QSerialPort tty_port;
static QList<QSerialPortInfo> ports; static QList<QSerialPortInfo> ports;
struct motion_data_t { /*static struct motion_data_t motion_data[MOTION_DATA_SIZE] = {
uint16_t pos;
int16_t speed;
};
#define MOTION_DATA_SIZE (16U)
static struct motion_data_t motion_data[MOTION_DATA_SIZE] = {
{192U, 128}, {160U, 128}, {128U, 128}, {96U, 128}, {192U, 128}, {160U, 128}, {128U, 128}, {96U, 128},
{0U, 256}, {704U, 256}, {640U, 256}, {576U, 256}, {0U, 256}, {704U, 256}, {640U, 256}, {576U, 256},
{512U, 256}, {448U, 256}, {384U, 256}, {352U, 256}, {512U, 256}, {448U, 256}, {384U, 256}, {352U, 256},
{320U, 128}, {288U, 128}, {256U, 128}, {224U, 128}}; {320U, 128}, {288U, 128}, {256U, 128}, {224U, 128}};*/
static bool port_opened = false; static bool port_opened = false;
...@@ -31,6 +24,8 @@ struct pid_log { ...@@ -31,6 +24,8 @@ struct pid_log {
static QByteArray arr; static QByteArray arr;
static uint32_t m_pos[6U] = {0, 8, 8, 0, 0, 8};
MainWindow::MainWindow(QWidget *parent) MainWindow::MainWindow(QWidget *parent)
: QMainWindow(parent), ui(new Ui::MainWindow) : QMainWindow(parent), ui(new Ui::MainWindow)
{ {
...@@ -57,6 +52,28 @@ MainWindow::MainWindow(QWidget *parent) ...@@ -57,6 +52,28 @@ MainWindow::MainWindow(QWidget *parent)
QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timer_event())); QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timer_event()));
uint32_t i;
static struct motion_data_t md[MOTION_DATA_SIZE] = {
{ui->lineEdit, 180U, 128, 0}, {ui->lineEdit_2, 165U, 128, 0},
{ui->lineEdit_3, 150, 128, 0}, {ui->lineEdit_4, 135, 128, 0},
{ui->lineEdit_5, 80, 256, 0}, {ui->lineEdit_6, 700, 256, 0},
{ui->lineEdit_7, 570, 256, 0}, {ui->lineEdit_8, 450, 256, 0},
{ui->lineEdit_9, 360, 256, 0}, {ui->lineEdit_10, 330, 256, 0},
{ui->lineEdit_11, 300, 256, 0}, {ui->lineEdit_12, 270, 256, 0},
{ui->lineEdit_13, 240, 128, 0}, {ui->lineEdit_14, 225, 128, 0},
{ui->lineEdit_15, 210, 128, 0}, {ui->lineEdit_16, 195, 128, 0}};
motion_data = md;
for (i = 0; i < MOTION_DATA_SIZE; i++) {
md[i].edit->setText(tr("%i").setNum(md[i].pos));
connect(md[i].edit, &QLineEdit::textChanged, [=](void) {
md[i].pos = (uint16_t)md[i].edit->text().toUInt();
});
}
t = 0U; t = 0U;
timer->start(); timer->start();
} }
...@@ -309,7 +326,7 @@ make_crc(struct can_packet_t *msg) ...@@ -309,7 +326,7 @@ make_crc(struct can_packet_t *msg)
void void
MainWindow::sendMsg(struct can_packet_t *msg) MainWindow::sendMsg(struct can_packet_t *msg)
{ {
printMsg(msg, 1); // printMsg(msg, 1);
tty_port.write(reinterpret_cast<char *>(msg), tty_port.write(reinterpret_cast<char *>(msg),
sizeof(struct can_packet_t)); sizeof(struct can_packet_t));
...@@ -336,6 +353,17 @@ MainWindow::on_btnCalibrateAll_clicked() ...@@ -336,6 +353,17 @@ MainWindow::on_btnCalibrateAll_clicked()
} }
} }
static inline int16_t
diff(uint16_t v1, uint16_t v2)
{
int32_t d = abs(v2 - v1);
if (d > 384) {
d = 768 - d;
}
return (int16_t)d;
}
void void
MainWindow::timer_event() MainWindow::timer_event()
{ {
...@@ -403,42 +431,10 @@ MainWindow::timer_event() ...@@ -403,42 +431,10 @@ MainWindow::timer_event()
} }
}*/ }*/
if (t % 12U < 6U) { if (t % 13U == 0U) {
static uint32_t m_pos[6U] = {0, 8, 8, 0, 0, 8}; if (drive_moving[0]) {
make_step();
uint8_t d = t % 12U;
// for (d = 0U; d < 6U; d++) {
if (drive_moving[d]) {
uint16_t pos =
motion_data[m_pos[d] % MOTION_DATA_SIZE]
.pos;
int16_t speed =
motion_data[m_pos[d] % MOTION_DATA_SIZE]
.speed;
struct can_packet_t msg;
msg.magic = CAN_MSG_MAGIC;
struct motion_cmd_t data;
data.drive = d & 0x1;
data.motion_type = MOTION_TYPE_POS;
data.position = pos;
data.speed = speed;
msg.msg.src_id = 0x10;
msg.msg.dest_id = (d / 2U) + 1U;
msg.msg.cmd_id = MSG_ID_DRV_LEG_MOTION;
msg.bus = 0U;
msg.len = sizeof(data);
memcpy(msg.data, &data, sizeof(data));
make_crc(&msg);
sendMsg(&msg);
m_pos[d]++;
} }
//}
} }
} }
...@@ -614,3 +610,49 @@ MainWindow::on_sliderSpeed_valueChanged(int value) ...@@ -614,3 +610,49 @@ MainWindow::on_sliderSpeed_valueChanged(int value)
QString str; QString str;
ui->labelSpeed->setText(str.setNum(value)); ui->labelSpeed->setText(str.setNum(value));
} }
void
MainWindow::make_step()
{
uint32_t d;
ui->label->setText(tr("%u").setNum(m_pos[0] % MOTION_DATA_SIZE));
for (d = 0U; d < 6U; d++) {
uint16_t pos = motion_data[m_pos[d] % MOTION_DATA_SIZE].pos;
int16_t speed = motion_data[m_pos[d] % MOTION_DATA_SIZE].speed;
speed =
diff(motion_data[m_pos[d] % MOTION_DATA_SIZE].pos,
motion_data[(m_pos[d] - 1U) % MOTION_DATA_SIZE].pos);
speed *= 4U;
struct can_packet_t msg;
msg.magic = CAN_MSG_MAGIC;
struct motion_cmd_t data;
data.drive = d & 0x1;
data.motion_type = MOTION_TYPE_POS;
data.position = pos;
data.speed = speed;
msg.msg.src_id = 0x10;
msg.msg.dest_id = (d / 2U) + 1U;
msg.msg.cmd_id = MSG_ID_DRV_LEG_MOTION;
msg.bus = 0U;
msg.len = sizeof(data);
memcpy(msg.data, &data, sizeof(data));
make_crc(&msg);
sendMsg(&msg);
m_pos[d]++;
}
}
void
MainWindow::on_pushButton_clicked()
{
make_step();
}
#ifndef MAINWINDOW_H #ifndef MAINWINDOW_H
#define MAINWINDOW_H #define MAINWINDOW_H
#include <QLineEdit>
#include <QMainWindow> #include <QMainWindow>
#include <QTimer> #include <QTimer>
...@@ -22,6 +23,8 @@ namespace Ui ...@@ -22,6 +23,8 @@ namespace Ui
class MainWindow; class MainWindow;
} }
#define MOTION_DATA_SIZE (16U)
class MainWindow : public QMainWindow class MainWindow : public QMainWindow
{ {
Q_OBJECT Q_OBJECT
...@@ -87,6 +90,8 @@ class MainWindow : public QMainWindow ...@@ -87,6 +90,8 @@ class MainWindow : public QMainWindow
void timer_event(); void timer_event();
void on_pushButton_clicked();
private: private:
Ui::MainWindow *ui; Ui::MainWindow *ui;
QTimer *timer; QTimer *timer;
...@@ -96,6 +101,17 @@ class MainWindow : public QMainWindow ...@@ -96,6 +101,17 @@ class MainWindow : public QMainWindow
bool m_presented[3]; bool m_presented[3];
bool drive_moving[6]; bool drive_moving[6];
uint32_t m_ping[3]; uint32_t m_ping[3];
struct motion_data_t {
QLineEdit *edit;
uint16_t pos;
int16_t speed;
uint32_t __pad;
};
struct motion_data_t *motion_data;
void make_step(void);
}; };
#endif // MAINWINDOW_H #endif // MAINWINDOW_H
...@@ -392,8 +392,8 @@ ...@@ -392,8 +392,8 @@
<widget class="QLabel" name="label"> <widget class="QLabel" name="label">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>530</x> <x>10</x>
<y>440</y> <y>420</y>
<width>60</width> <width>60</width>
<height>16</height> <height>16</height>
</rect> </rect>
...@@ -402,6 +402,179 @@ ...@@ -402,6 +402,179 @@
<string>TextLabel</string> <string>TextLabel</string>
</property> </property>
</widget> </widget>
<widget class="QLineEdit" name="lineEdit">
<property name="geometry">
<rect>
<x>10</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_2">
<property name="geometry">
<rect>
<x>80</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_3">
<property name="geometry">
<rect>
<x>150</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_4">
<property name="geometry">
<rect>
<x>220</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_5">
<property name="geometry">
<rect>
<x>290</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_6">
<property name="geometry">
<rect>
<x>360</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_7">
<property name="geometry">
<rect>
<x>430</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_8">
<property name="geometry">
<rect>
<x>500</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_9">
<property name="geometry">
<rect>
<x>570</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_10">
<property name="geometry">
<rect>
<x>640</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_11">
<property name="geometry">
<rect>
<x>710</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_12">
<property name="geometry">
<rect>
<x>780</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_13">
<property name="geometry">
<rect>
<x>850</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_14">
<property name="geometry">
<rect>
<x>920</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_15">
<property name="geometry">
<rect>
<x>990</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="lineEdit_16">
<property name="geometry">
<rect>
<x>1060</x>
<y>390</y>
<width>61</width>
<height>24</height>
</rect>
</property>
</widget>
<widget class="QPushButton" name="pushButton">
<property name="geometry">
<rect>
<x>170</x>
<y>320</y>
<width>151</width>
<height>61</height>
</rect>
</property>
<property name="text">
<string>шаг</string>
</property>
</widget>
</widget> </widget>
</item> </item>
<item> <item>
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment