00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "PreCompiled.h"
00025
00026 #ifndef _PreComp_
00027 #endif
00028
00029 #include "ui_TaskTrajectory.h"
00030 #include "TaskTrajectory.h"
00031 #include <Gui/Application.h>
00032 #include <Gui/Document.h>
00033 #include <Gui/BitmapFactory.h>
00034 #include <Gui/ViewProvider.h>
00035 #include <Gui/WaitCursor.h>
00036 #include <Base/Console.h>
00037 #include <Gui/Selection.h>
00038
00039
00040 using namespace RobotGui;
00041 using namespace Gui;
00042
00043 TaskTrajectory::TaskTrajectory(Robot::RobotObject *pcRobotObject,Robot::TrajectoryObject *pcTrajectoryObject,QWidget *parent)
00044 : TaskBox(Gui::BitmapFactory().pixmap("document-new"),tr("Trajectory"),true, parent),
00045 sim(pcTrajectoryObject->Trajectory.getValue(),pcRobotObject->getRobot()),
00046 pcRobot(pcRobotObject),
00047 Run(false),
00048 block(false),
00049 timePos(0.0)
00050 {
00051
00052 proxy = new QWidget(this);
00053 ui = new Ui_TaskTrajectory();
00054 ui->setupUi(proxy);
00055 QMetaObject::connectSlotsByName(this);
00056
00057 this->groupLayout()->addWidget(proxy);
00058
00059
00060 sim.Tool = pcRobotObject->Tool.getValue();
00061
00062 ui->trajectoryTable->setSortingEnabled(false);
00063
00064 Robot::Trajectory trac = pcTrajectoryObject->Trajectory.getValue();
00065 ui->trajectoryTable->setRowCount(trac.getSize());
00066 duration = trac.getDuration();
00067 ui->timeSpinBox->setMaximum(duration);
00068
00069 for(unsigned int i=0;i<trac.getSize();i++){
00070 Robot::Waypoint pt = trac.getWaypoint(i);
00071 switch(pt.Type){
00072 case Robot::Waypoint::UNDEF: ui->trajectoryTable->setItem(i, 0, new QTableWidgetItem(QString::fromAscii("UNDEF")));break;
00073 case Robot::Waypoint::CIRC: ui->trajectoryTable->setItem(i, 0, new QTableWidgetItem(QString::fromAscii("CIRC")));break;
00074 case Robot::Waypoint::PTP: ui->trajectoryTable->setItem(i, 0, new QTableWidgetItem(QString::fromAscii("PTP")));break;
00075 case Robot::Waypoint::LINE: ui->trajectoryTable->setItem(i, 0, new QTableWidgetItem(QString::fromAscii("LIN")));break;
00076 default: ui->trajectoryTable->setItem(i, 0, new QTableWidgetItem(QString::fromAscii("UNDEF")));break;
00077 }
00078 ui->trajectoryTable->setItem(i, 1, new QTableWidgetItem(QString::fromUtf8(pt.Name.c_str())));
00079 if(pt.Cont)
00080 ui->trajectoryTable->setItem(i, 2, new QTableWidgetItem(QString::fromAscii("|")));
00081 else
00082 ui->trajectoryTable->setItem(i, 2, new QTableWidgetItem(QString::fromAscii("-")));
00083 ui->trajectoryTable->setItem(i, 3, new QTableWidgetItem(QString::number(pt.Velocity)));
00084 ui->trajectoryTable->setItem(i, 4, new QTableWidgetItem(QString::number(pt.Accelaration)));
00085
00086 }
00087
00088 QObject::connect(ui->ButtonStepStart ,SIGNAL(clicked()),this,SLOT(start()));
00089 QObject::connect(ui->ButtonStepStop ,SIGNAL(clicked()),this,SLOT(stop()));
00090 QObject::connect(ui->ButtonStepRun ,SIGNAL(clicked()),this,SLOT(run()));
00091 QObject::connect(ui->ButtonStepBack ,SIGNAL(clicked()),this,SLOT(back()));
00092 QObject::connect(ui->ButtonStepForward ,SIGNAL(clicked()),this,SLOT(forward()));
00093 QObject::connect(ui->ButtonStepEnd ,SIGNAL(clicked()),this,SLOT(end()));
00094
00095
00096
00097 timer = new QTimer( this );
00098 timer->setInterval(100);
00099 QObject::connect(timer ,SIGNAL(timeout ()),this,SLOT(timerDone()));
00100
00101 QObject::connect( ui->timeSpinBox ,SIGNAL(valueChanged(double)), this, SLOT(valueChanged(double)) );
00102 QObject::connect( ui->timeSlider ,SIGNAL(valueChanged(int) ), this, SLOT(valueChanged(int)) );
00103
00104
00105 ViewProv = dynamic_cast<ViewProviderRobotObject*>(Gui::Application::Instance->activeDocument()->getViewProvider(pcRobotObject) );
00106
00107 setTo();
00108 }
00109
00110 TaskTrajectory::~TaskTrajectory()
00111 {
00112 delete ui;
00113
00114 }
00115
00116 void TaskTrajectory::viewTool(const Base::Placement pos)
00117 {
00118 double A,B,C;
00119 pos.getRotation().getYawPitchRoll(A,B,C);
00120
00121 QString result = QString::fromAscii("Pos:(%1, %2, %3, %4, %5, %6)")
00122 .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Length,pos.getPosition().x),0,'f',1)
00123 .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Length,pos.getPosition().y),0,'f',1)
00124 .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Length,pos.getPosition().z),0,'f',1)
00125 .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Angle,A),0,'f',1)
00126 .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Angle,B),0,'f',1)
00127 .arg(Base::UnitsApi::toDblWithUserPrefs(Base::Angle,C),0,'f',1);
00128
00129 ui->label_Pos->setText(result);
00130 }
00131
00132 void TaskTrajectory::setTo(void)
00133 {
00134 sim.Tool = pcRobot->Tool.getValue();
00135
00136 if(timePos < 0.0001)
00137 sim.reset();
00138 else{
00139 sim.setToTime(timePos);
00140 }
00141 ViewProv->setAxisTo(sim.Axis[0],sim.Axis[1],sim.Axis[2],sim.Axis[3],sim.Axis[4],sim.Axis[5],sim.Rob.getTcp());
00142 axisChanged(sim.Axis[0],sim.Axis[1],sim.Axis[2],sim.Axis[3],sim.Axis[4],sim.Axis[5],sim.Rob.getTcp());
00143 viewTool(sim.Rob.getTcp());
00144 }
00145
00146 void TaskTrajectory::start(void)
00147 {
00148 timePos = 0.0f;
00149 ui->timeSpinBox->setValue(timePos);
00150 ui->timeSlider->setValue(int((timePos/duration)*1000));
00151 setTo();
00152
00153 }
00154 void TaskTrajectory::stop(void)
00155 {
00156 timer->stop();
00157 Run = false;
00158 }
00159 void TaskTrajectory::run(void)
00160 {
00161 timer->start();
00162 Run = true;
00163 }
00164 void TaskTrajectory::back(void)
00165 {
00166 }
00167 void TaskTrajectory::forward(void)
00168 {
00169 }
00170 void TaskTrajectory::end(void)
00171 {
00172 timePos = duration;
00173 ui->timeSpinBox->setValue(timePos);
00174 ui->timeSlider->setValue(int((timePos/duration)*1000));
00175 setTo();
00176 }
00177
00178 void TaskTrajectory::timerDone(void)
00179 {
00180 if(timePos < duration){
00181 timePos += .1f;
00182 ui->timeSpinBox->setValue(timePos);
00183 ui->timeSlider->setValue(int((timePos/duration)*1000));
00184 setTo();
00185 timer->start();
00186 }else{
00187 timer->stop();
00188 Run = false;
00189 }
00190 }
00191
00192 void TaskTrajectory::valueChanged ( int value )
00193 {
00194 if(!block){
00195 timePos = duration*(value/1000.0);
00196 block=true;
00197 ui->timeSpinBox->setValue(timePos);
00198 block=false;
00199 setTo();
00200 }
00201 }
00202
00203 void TaskTrajectory::valueChanged ( double value )
00204 {
00205 if(!block){
00206 timePos = value;
00207 block=true;
00208 ui->timeSlider->setValue(int((timePos/duration)*1000));
00209 block=false;
00210 setTo();
00211 }
00212 }
00213
00214
00215 #include "moc_TaskTrajectory.cpp"