F:\学科、技能、编程\【编程-文件\proj\舵机吸附
F:\学科、技能、编程\【编程-文件\proj\舵机吸附
根据DynamixelSDK指定串口数据帧的通信协议；使用循环冗余校验；实现三个舵机关节的点动控制和连续运动；保存关键帧；
#include "mainwindow.h" #include "ui_mainwindow.h" // MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow) { ui->setupUi(this); ui->closeMyComBtn->setEnabled(false); //开始“关闭串口”按钮不可用 ui->sendMsgBtn->setEnabled(false); //开始“发送数据”按钮不可用 ui->sendMsgBtn->setEnabled(false);//发送数据按钮不可用 ui->pushButton_serial->setEnabled(false);//发送数据按钮不可用 connect(this, SIGNAL(valueChanged()),this, SLOT(on_sendMsgBtn_clicked()));//连接信号与槽---发送一帧数据 connect(this,SIGNAL(new_item()),this,SLOT(fresh_item()));//连接信号与槽，更新右边项； for(int i=0;i<8;i++) //初始化 item { for(int j=0;j<3;j++) { item[i][j]=0; } } //菜单响应 saveAction = new QAction(tr("&save"), this); //saveAction->setShortcut(QKeySequence::Open); //快捷键 //saveAction->setStatusTip(tr("Open a file.")); //状态栏提示语 helpAction = new QAction(tr("&help"), this); file = menuBar()->addMenu(tr("&File")); //创建菜单项 file->addAction(saveAction); //添加动作 Help = menuBar()->addMenu(tr("&Help")); //创建菜单项 Help->addAction(helpAction); //添加动作 connect(saveAction, SIGNAL(triggered()), this, SLOT(save())); connect(helpAction, SIGNAL(triggered()), this, SLOT(help())); } // 析构函数 MainWindow::~MainWindow() { delete ui; } //读串口函数//////////////////// void MainWindow::readMyCom()//读串口函数 { QByteArray tmp = myCom->readAll(); // QDataStream out(&tmp,QIODevice::ReadWrite); int size=tmp.size(); QString strHex; for(int i =0;i < size;i++) { qint8 outChar = tmp[i]; QString str = QString("%1").arg(outChar&0xFF,2,16,QLatin1Char('0')); strHex +=str; } QString string="\n";//换行 strHex +=string; ui->textBrowser->insertPlainText(strHex); //读取串口缓冲区的所有数据给临时变量temp // ui->textBrowser->insertPlainText(temp); //将串口的数据显示在窗口的文本浏览器中 } void MainWindow::on_openMyComBtn_clicked() { QString portName = ui->portNameComboBox->currentText(); //获取串口名 myCom = new Win_QextSerialPort(portName,QextSerialBase::EventDriven); //定义串口对象，并传递参数，在构造函数里对其进行初始化 myCom ->open(QIODevice::ReadWrite); //打开串口 if(ui->baudRateComboBox->currentText()==tr("9600")) //根据组合框内容对串口进行设置 myCom->setBaudRate(BAUD9600); else if(ui->baudRateComboBox->currentText()==tr("115200")) myCom->setBaudRate(BAUD115200); if(ui->dataBitsComboBox->currentText()==tr("8")) myCom->setDataBits(DATA_8); else if(ui->dataBitsComboBox->currentText()==tr("7")) myCom->setDataBits(DATA_7); if(ui->parityComboBox->currentText()==tr("无")) myCom->setParity(PAR_NONE); else if(ui->parityComboBox->currentText()==tr("奇")) myCom->setParity(PAR_ODD); else if(ui->parityComboBox->currentText()==tr("偶")) myCom->setParity(PAR_EVEN); if(ui->stopBitsComboBox->currentText()==tr("1")) myCom->setStopBits(STOP_1); else if(ui->stopBitsComboBox->currentText()==tr("2")) myCom->setStopBits(STOP_2); myCom->setFlowControl(FLOW_OFF); myCom->setTimeout(500); connect(myCom,SIGNAL(readyRead()),this,SLOT(readMyCom())); //信号和槽函数关联，当串口缓冲区有数据时，进行读串口操作 //connect(dial,SIGNAL("valueChanged(int)"),this, SLOT(on_dial_valueChanged())); ui->openMyComBtn->setEnabled(false); //打开串口后“打开串口”按钮不可用 ui->closeMyComBtn->setEnabled(true); //打开串口后“关闭串口”按钮可用 ui->sendMsgBtn->setEnabled(true); //打开串口后“发送数据”按钮可用 ui->sendMsgBtn->setEnabled(true); //发送数据按钮可用 ui->pushButton_serial->setEnabled(true);//发送数据按钮可用 ui->baudRateComboBox->setEnabled(false); //设置各个组合框不可用 ui->dataBitsComboBox->setEnabled(false); ui->parityComboBox->setEnabled(false); ui->stopBitsComboBox->setEnabled(false); ui->portNameComboBox->setEnabled(false); } void MainWindow::on_closeMyComBtn_clicked() { myCom->close(); ui->openMyComBtn->setEnabled(true); //关闭串口后“打开串口”按钮可用 ui->closeMyComBtn->setEnabled(false); //关闭串口后“关闭串口”按钮不可用 ui->sendMsgBtn->setEnabled(false); //关闭串口后“发送数据”按钮不可用 ui->baudRateComboBox->setEnabled(true); //设置各个组合框可用 ui->dataBitsComboBox->setEnabled(true); ui->parityComboBox->setEnabled(true); ui->stopBitsComboBox->setEnabled(true); ui->portNameComboBox->setEnabled(true); } //////更改ID////////////////////////////////// void MainWindow::on_IDchange_clicked() { // int ID=ui->ID_value->text().toInt();//有效性检验 int ID1=ui->ID_old->text().toInt();//有效性判断 int ID2=ui->ID_value->text().toInt(); QByteArray data; data.resize(8); data[0] = 0xff; data[1] = 0xff; data[2] = (unsigned char) (ID1& 0x00ff); data[3] = 0x04;// data[4] = 0x03;// data[5] = 0x03;// data[6]= (unsigned char) (ID2& 0x00ff); // data[6] = (unsigned char) (ID& 0x00ff); unsigned char crc = 0; int i; for (i = 2; i <7; i++) { crc += data[i]; } data[7] = ~crc; myCom->write(( const char* )data,8*sizeof(unsigned char)); //显示发送的数据 QByteArray tmp = data; // QDataStream out(&tmp,QIODevice::ReadWrite); int size=tmp.size(); QString strHex; for(int i =0;i < size;i++) { qint8 outChar = tmp[i]; QString str = QString("%1").arg(outChar&0xFF,2,16,QLatin1Char('0')); strHex +=str; } QString string="\n";//换行 strHex +=string; ui->textBrowser_2->insertPlainText(strHex); } //////发送一串信号，控制5个舵机////////////////////////////////// void MainWindow::on_sendMsgBtn_clicked() { // myCom->write(ui->sendMsgLineEdit->text().toAscii()); //以ASCII码形式将数据写入串口 int ID[5]; //以实际情况而定 ID[0]=ui->ID1->text().toInt();//有效性判断 ID[1]=ui->ID2->text().toInt(); ID[2]=ui->ID3->text().toInt(); ID[3]=ui->ID4->text().toInt(); ID[4]=ui->ID5->text().toInt(); //dial和edit同步 QString string; int ang=0; ang=ui->dial->value(); string = string.setNum(ang); ui->position1->setText(string); ang=ui->dial_2->value(); string = string.setNum(ang); ui->position2->setText(string); ang=ui->dial_3->value(); string = string.setNum(ang); ui->position3->setText(string); int angle[5]; angle[0]=ui->position1->text().toInt(); angle[1]=ui->position2->text().toInt(); angle[2]=ui->position3->text().toInt(); angle[3]=1024-ui->position3->text().toInt(); angle[4]=1024-ui->position1->text().toInt(); int speed[5]; speed[0]=ui->speed1->text().toInt(); speed[1]=ui->speed2->text().toInt(); speed[2]=ui->speed3->text().toInt(); speed[3]=ui->speed3->text().toInt(); speed[4]=ui->speed1->text().toInt(); QByteArray data; data.resize(28+5); data[0] = 0xff; data[1] = 0xff; data[2] = 0xfe; data[3] = 0x18+0x05;//Length (L+1)*N+4(L:Data length for each Dynamixel actuator, N: The number of Dynamixel actuators) data[4] = 0x83;//用于同时控制多个 Dynamixel 舵机 data[5] = 0x1e;//数据被写入的起始地址 data[6] = 0x04;//L=4,后面是ID+L个数共N组 int i; for (i = 0; i < 5; i++)//4个舵机 { data[7 + 5 * i] = (unsigned char) (ID[i] & 0x00ff); data[8 + 5 * i] = (unsigned char) (angle[i] & 0x00ff); data[9 + 5 * i] = (unsigned char) ((angle[i] >> 8) & 0x00ff); data[10 + 5 * i] = (unsigned char) (speed[i] & 0x00ff); data[11 + 5 * i] = (unsigned char) ((speed[i] >> 8) & 0x00ff); } unsigned char crc = 0; for (i = 2; i < 27+5; i++) { crc += data[i]; } data[27+5] = ~crc; myCom->write(( const char* )data,(28+5)*sizeof(unsigned char)); //显示发送的数据 QByteArray tmp = data; // QDataStream out(&tmp,QIODevice::ReadWrite); int size=tmp.size(); QString strHex; for(int i =0;i < size;i++) { qint8 outChar = tmp[i]; QString str = QString("%1").arg(outChar&0xFF,2,16,QLatin1Char('0')); strHex +=str; } QString string1="\n";//换行 strHex +=string1; ui->textBrowser_2->insertPlainText(strHex); } //////手动旋转舵机///////////////////////////////// void MainWindow::on_dial_dialReleased() { emit valueChanged(); } void MainWindow::on_dial_2_dialReleased() { emit valueChanged(); } void MainWindow::on_dial_3_dialReleased() { emit valueChanged(); } /////edit与旋钮的数据同步 void MainWindow::on_position1_textChanged(QString ) { QString string; string =ui->position1->text(); int num=string.toInt(); ui->dial->setValue(num); } void MainWindow::on_position2_textChanged(QString ) { QString string; string =ui->position2->text(); int num=string.toInt(); ui->dial_2->setValue(num); } void MainWindow::on_position3_textChanged(QString ) { QString string; string =ui->position3->text(); int num=string.toInt(); ui->dial_3->setValue(num); } //记录到右边项 void MainWindow::on_register_2_clicked() { int index=ui->spinBox3->value();//item项的序号 QString string; int jd; string =ui->position1->text(); jd =string.toInt(); item[index][0]=jd; string =ui->position2->text(); jd=string.toInt(); item[index][1]=jd; string =ui->position3->text(); jd=string.toInt(); item[index][2]=jd; emit new_item(); //发射信号，记录到右边项 } //槽函数，更新显示 记录 到右边项 void MainWindow::fresh_item() { QString string; string = string.setNum(item[0][0]); ui->jd01->setText(string); string = string.setNum(item[0][1]); ui->jd02->setText(string); string = string.setNum(item[0][2]); ui->jd03->setText(string); string = string.setNum(item[1][0]); ui->jd11->setText(string); string = string.setNum(item[1][1]); ui->jd12->setText(string); string = string.setNum(item[1][2]); ui->jd13->setText(string); string = string.setNum(item[2][0]); ui->jd21->setText(string); string = string.setNum(item[2][1]); ui->jd22->setText(string); string = string.setNum(item[2][2]); ui->jd23->setText(string); string = string.setNum(item[3][0]); ui->jd31->setText(string); string = string.setNum(item[3][1]); ui->jd32->setText(string); string = string.setNum(item[3][2]); ui->jd33->setText(string); string = string.setNum(item[4][0]); ui->jd41->setText(string); string = string.setNum(item[4][1]); ui->jd42->setText(string); string = string.setNum(item[4][2]); ui->jd43->setText(string); string = string.setNum(item[5][0]); ui->jd51->setText(string); string = string.setNum(item[5][1]); ui->jd52->setText(string); string = string.setNum(item[5][2]); ui->jd53->setText(string); string = string.setNum(item[6][0]); ui->jd61->setText(string); string = string.setNum(item[6][1]); ui->jd62->setText(string); string = string.setNum(item[6][2]); ui->jd63->setText(string); string = string.setNum(item[7][0]); ui->jd71->setText(string); string = string.setNum(item[7][1]); ui->jd72->setText(string); string = string.setNum(item[7][2]); ui->jd73->setText(string); } //--以下这个代码主要是实现以毫秒为单位的延时------->为move服务 void MainWindow::sleep(unsigned int msec) { // QWaitCondition waitCond; // //waitCond.wait (msec); QTime dieTime = QTime::currentTime().addMSecs(msec); while( QTime::currentTime() < dieTime ) QCoreApplication::processEvents(QEventLoop::AllEvents, 100); } //--生成两个角度之间的连续动作------->为on_pushButton_serial_clicked服务 void MainWindow::move(unsigned int a1,unsigned int a2,unsigned int a3,unsigned int b1,unsigned int b2,unsigned int b3) { int NUM=20; double step1=(b1-a1)/NUM; double step2=(b2-a2)/NUM; double step3=(b3-a3)/NUM; int jd[NUM+1][3];//细分之后的角度 int i=0; for(i=0;i<NUM+1;i++) //获得角度值 { jd[i][0]=int(a1+step1*(i)); jd[i][1]=int(a2+step2*(i)); jd[i][2]=int(a3+step3*(i)); } for(i=0;i<NUM+1;i++) // 发送角度值 { //延时80毫秒 sleep(80); //获取ID值 int ID[5]; //以实际情况而定 ID[0]=ui->ID1->text().toInt();//有效性判断 ID[1]=ui->ID2->text().toInt(); ID[2]=ui->ID3->text().toInt(); ID[3]=ui->ID4->text().toInt(); ID[4]=ui->ID5->text().toInt(); //角度值 int angle[5]; angle[0]=jd[i][0]; angle[1]=jd[i][1]; angle[2]=jd[i][2]; angle[3]=1024-jd[i][2]; angle[4]=1024-jd[i][0]; //速度值 int speed[5]; speed[0]=ui->speed1->text().toInt(); speed[1]=ui->speed2->text().toInt(); speed[2]=ui->speed3->text().toInt(); speed[3]=ui->speed3->text().toInt(); speed[4]=ui->speed1->text().toInt(); //发送数据 QByteArray data; data.resize(28+5); data[0] = 0xff; data[1] = 0xff; data[2] = 0xfe; data[3] = 0x18+0x05; //Length (L+1)*N+4(L:Data length for each Dynamixel actuator, N: The number of Dynamixel actuators) data[4] = 0x83; //用于同时控制多个 Dynamixel 舵机 data[5] = 0x1e; //数据被写入的起始地址 data[6] = 0x04; //L=4,后面是ID+L个数共N组 int i; for (i = 0; i < 5; i++) //5个舵机 { data[7 + 5 * i] = (unsigned char) (ID[i] & 0x00ff); data[8 + 5 * i] = (unsigned char) (angle[i] & 0x00ff); data[9 + 5 * i] = (unsigned char) ((angle[i] >> 8) & 0x00ff); data[10 + 5 * i] = (unsigned char) (speed[i] & 0x00ff); data[11 + 5 * i] = (unsigned char) ((speed[i] >> 8) & 0x00ff); } unsigned char crc = 0; for (i = 2; i < 27+5; i++) //生成校验码 { crc += data[i]; } data[27+5] = ~crc; //生成校验码 myCom->write(( const char* )data,(28+5)*sizeof(unsigned char)); //写入数据 //显示发送的数据 QByteArray tmp = data; // QDataStream out(&tmp,QIODevice::ReadWrite); int size=tmp.size(); QString strHex; for(int i =0;i < size;i++) //转换为16进制字符 { qint8 outChar = tmp[i]; QString str = QString("%1").arg(outChar&0xFF,2,16,QLatin1Char('0')); strHex +=str; } QString string1="\n";//换行 strHex +=string1; ui->textBrowser_2->insertPlainText(strHex); //ui->textBrowser_2->clear(); //清除显示的数据 } } //发送连续序列 void MainWindow::on_pushButton_serial_clicked() { unsigned int ind1=ui->spinBox1->value(); unsigned int ind2=ui->spinBox2->value(); bool panduan = 0;//作为判断之用 for(unsigned int i=ind1;i<ind2;i++)//判断个数，防止出错 for(unsigned int j=0;j<3;j++) { int temp = item[i][j]; if(temp == 0) panduan = 1; } if(panduan == 1) { QMessageBox::information(NULL, "Title", "数据有错！", QMessageBox::Yes , QMessageBox::Yes); return ; } for(unsigned int i=ind1;i<ind2;i++)//连续发送 { move(item[i][0],item[i][1],item[i][2],item[i+1][0],item[i+1][1],item[i+1][2]); } } // 清除textBrowser显示的数据 void MainWindow::on_pushButtoncleandata_clicked() { ui->textBrowser_2->clear(); //清除显示的数据 ui->textBrowser->clear(); //清除显示的数据 } //保存菜单的动作响应槽函数 void MainWindow::save() { //保存数据 QFile file("关节数据.txt"); if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) return; QTextStream out(&file); unsigned int ind1=ui->spinBox1->value(); unsigned int ind2=ui->spinBox2->value(); cout<<" 数据项 "<<ind1<<" "<<" 到 "<<" "<<ind2<<endl<<endl; for(unsigned int i=ind1;i<ind2+1;i++)// { cout<<"item "<<i<<" : "; for(unsigned int j=0;j<3;j++) { cout<<item[i][j]<<" "; } cout<<endl; } // // out << "The magic number is: " << 49 << "\n"; // QMessageBox::about(NULL, "保存数据成功", "关节数据成功保存至<font color='red'>关节数据.txt</font>"); } void MainWindow::help() //帮助菜单的动作响应槽函数 { QString local_path = QString("Help.txt"); //a.txt、a.exe、a.mp3、a.mp4、a.rmvb等 // QString path = QString("file:///") + local_path; QDesktopServices::openUrl(QUrl(local_path, QUrl::TolerantMode)); //打开帮助文档 }
