Commit 358c5b5b authored by Alexey Hohlov's avatar Alexey Hohlov

Набросок для отладки алгоритма движения

parent 3d40338b
......@@ -8,6 +8,19 @@
static QSerialPort tty_port;
static QList<QSerialPortInfo> ports;
struct motion_data_t {
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},
{0U, 256}, {704U, 256}, {640U, 256}, {576U, 256},
{512U, 256}, {448U, 256}, {384U, 256}, {352U, 256},
{320U, 128}, {288U, 128}, {256U, 128}, {224U, 128}};
static bool port_opened = false;
struct pid_log {
......@@ -24,7 +37,8 @@ MainWindow::MainWindow(QWidget *parent)
ui->setupUi(this);
timer = new QTimer();
timer->setInterval(10);
timer->setInterval(20);
timer->setTimerType(Qt::PreciseTimer);
QList<qint32> baud = QSerialPortInfo::standardBaudRates();
for (int i = 0; i < baud.count(); i++) {
......@@ -77,6 +91,7 @@ MainWindow::on_connect_btn_clicked()
for (d = 0U; d < 6U; d++) {
disable_drive(d);
drive_moving[d] = false;
}
m_presented[0] = false;
m_presented[1] = false;
......@@ -272,7 +287,7 @@ MainWindow::readData()
arr += data;
while (find_msg(&msg, &arr)) {
printMsg(&msg, 0);
// printMsg(&msg, 0);
parseMsg(&msg);
}
}
......@@ -324,36 +339,112 @@ MainWindow::on_btnCalibrateAll_clicked()
void
MainWindow::timer_event()
{
// signal(SIGRTMIN, SIG_IGN);
if (port_opened) {
t++;
if (t == 100U) {
uint8_t d;
/* сброс счетчика */
if (t == 10000000U) {
t = 0U;
}
// if (t & 0x1U) {
if (t % 50U < 3U) {
uint8_t d = t % 50U;
/*for (d = 0U; d < 0x03U; d++) {*/
struct can_packet_t msg;
msg.magic = CAN_MSG_MAGIC;
msg.msg.src_id = 0x10;
msg.msg.dest_id = d + 1U;
msg.msg.cmd_id = MSG_ID_GET_STATUS;
msg.bus = 0U;
msg.len = 0U;
make_crc(&msg);
sendMsg(&msg);
m_ping[d]++;
if (m_ping[d] > 5U) {
disable_drive(d * 2U);
disable_drive(d * 2U + 1U);
m_presented[d] = false;
}
/*}*/
}
//}
/*if (t % 10U) {
if (drive_moving[5]) {
uint16_t pos =
(uint16_t)ui->sliderPosition->value();
int16_t speed = ui->sliderSpeed->value();
for (d = 0U; d < 0x03U; d++) {
struct can_packet_t msg;
msg.magic = CAN_MSG_MAGIC;
struct motion_cmd_t data;
data.drive = 1U;
data.motion_type = MOTION_TYPE_POS;
data.position = pos;
data.speed = speed;
msg.msg.src_id = 0x10;
msg.msg.dest_id = d + 1U;
msg.msg.cmd_id = MSG_ID_GET_STATUS;
msg.msg.dest_id = 0x3U;
msg.msg.cmd_id = MSG_ID_DRV_LEG_MOTION;
msg.bus = 0U;
msg.len = 0U;
msg.len = sizeof(data);
memcpy(msg.data, &data, sizeof(data));
make_crc(&msg);
sendMsg(&msg);
}
}*/
m_ping[d]++;
if (t % 12U < 6U) {
static uint32_t m_pos[6U] = {0, 8, 8, 0, 0, 8};
if (m_ping[d] > 5U) {
disable_drive(d * 2U);
disable_drive(d * 2U + 1U);
m_presented[d] = false;
}
}
uint8_t d = t % 12U;
t = 0U;
// 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]++;
}
//}
}
}
/*QString str;
ui->label->setText(str.setNum(t));*/
// signal(SIGRTMIN, SIG_DFL);
}
void
......@@ -378,7 +469,7 @@ MainWindow::calibrate_drv(uint8_t d)
msg.magic = CAN_MSG_MAGIC;
uint8_t dest = (d / 2U) + 1U;
uint8_t m = (d - 1U) & 0x1U;
uint8_t m = d & 0x1U;
msg.msg.dest_id = dest;
msg.msg.cmd_id = MSG_ID_DRV_CALIBRATION_START;
......@@ -398,7 +489,7 @@ MainWindow::parking_drv(uint8_t d)
msg.magic = CAN_MSG_MAGIC;
uint8_t dest = (d / 2U) + 1U;
uint8_t m = (d - 1U) & 0x1U;
uint8_t m = d & 0x1U;
msg.msg.dest_id = dest;
msg.msg.cmd_id = MSG_ID_DRV_LEG_STAND;
......@@ -481,3 +572,45 @@ MainWindow::on_btnParkM6_clicked()
{
parking_drv(5U);
}
void
MainWindow::on_btnMoving_toggled(bool checked)
{
uint8_t m;
for (m = 0U; m < 3U; m++) {
if (m_presented[m]) {
drive_moving[m * 2U] = checked;
drive_moving[m * 2U + 1U] = checked;
} else {
drive_moving[m * 2U] = false;
drive_moving[m * 2U + 1U] = false;
}
}
}
void
MainWindow::on_pushButton_6_clicked()
{
ui->sliderPosition->setValue(0);
}
void
MainWindow::on_pushButton_7_clicked()
{
ui->sliderSpeed->setValue(0);
}
void
MainWindow::on_sliderPosition_valueChanged(int value)
{
QString str;
ui->labelPos->setText(str.setNum(value));
}
void
MainWindow::on_sliderSpeed_valueChanged(int value)
{
QString str;
ui->labelSpeed->setText(str.setNum(value));
}
......@@ -43,8 +43,6 @@ class MainWindow : public QMainWindow
void printMsg(struct can_packet_t *msg, int dir);
void parseMsg(struct can_packet_t *msg);
void timer_event();
void on_btnParkAll_clicked();
void enable_drive(uint8_t d);
......@@ -77,6 +75,18 @@ class MainWindow : public QMainWindow
void on_btnParkM6_clicked();
void on_btnMoving_toggled(bool checked);
void on_pushButton_6_clicked();
void on_pushButton_7_clicked();
void on_sliderPosition_valueChanged(int value);
void on_sliderSpeed_valueChanged(int value);
void timer_event();
private:
Ui::MainWindow *ui;
QTimer *timer;
......@@ -84,6 +94,7 @@ class MainWindow : public QMainWindow
uint32_t t;
bool m_presented[3];
bool drive_moving[6];
uint32_t m_ping[3];
};
......
......@@ -179,7 +179,7 @@
<string>Калибровка 3</string>
</property>
</widget>
<widget class="QSlider" name="horizontalSlider">
<widget class="QSlider" name="sliderPosition">
<property name="geometry">
<rect>
<x>180</x>
......@@ -189,16 +189,16 @@
</rect>
</property>
<property name="minimum">
<number>-1536</number>
<number>0</number>
</property>
<property name="maximum">
<number>1536</number>
<number>768</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
<widget class="QSlider" name="horizontalSlider_2">
<widget class="QSlider" name="sliderSpeed">
<property name="geometry">
<rect>
<x>730</x>
......@@ -217,26 +217,26 @@
<enum>Qt::Horizontal</enum>
</property>
</widget>
<widget class="QPushButton" name="pushButton_6">
<widget class="QPushButton" name="btnZeroPos">
<property name="geometry">
<rect>
<x>370</x>
<y>30</y>
<width>91</width>
<height>25</height>
<height>21</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton_7">
<widget class="QPushButton" name="btnZeroSpeed">
<property name="geometry">
<rect>
<x>920</x>
<y>30</y>
<width>91</width>
<height>25</height>
<height>21</height>
</rect>
</property>
<property name="text">
......@@ -347,6 +347,61 @@
<string>Парковка всех</string>
</property>
</widget>
<widget class="QPushButton" name="btnMoving">
<property name="geometry">
<rect>
<x>10</x>
<y>320</y>
<width>151</width>
<height>61</height>
</rect>
</property>
<property name="text">
<string>Движение</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
</widget>
<widget class="QLabel" name="labelPos">
<property name="geometry">
<rect>
<x>480</x>
<y>30</y>
<width>71</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLabel" name="labelSpeed">
<property name="geometry">
<rect>
<x>1030</x>
<y>30</y>
<width>61</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>530</x>
<y>440</y>
<width>60</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>TextLabel</string>
</property>
</widget>
</widget>
</item>
<item>
......
......@@ -16,3 +16,6 @@ FORMS += \
INCLUDEPATH += \
/home/amper/coding/rhex/rhex_protocol/include
LIBS += \
-lrt
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