void GraphChart::handleKeys(int input){
switch(input){
case KEY_DOWN:
removePoint(m_chartPoints.at(0));
if(m_chartPoints.at(0)->m_Y < m_rows)
m_chartPoints.at(0)->m_Y++;
break;
case KEY_UP:
removePoint(m_chartPoints.at(0));
if(m_chartPoints.at(0)->m_Y > 0)
m_chartPoints.at(0)->m_Y--;
break;
case KEY_LEFT:
removePoint(m_chartPoints.at(0));
if(m_chartPoints.at(0)->m_X > 0)
m_chartPoints.at(0)->m_X--;
break;
case KEY_RIGHT:
removePoint(m_chartPoints.at(0));
if(m_chartPoints.at(0)->m_X < m_cols)
m_chartPoints.at(0)->m_X++;
break;
case '\n':
m_chartPoints.at(0)->m_hidden = !m_chartPoints.at(0)->m_hidden;
break;
}
}
void ConnectionManager::removeDuplicatePoints(Connection *c) {
bool removedSome = false;
bool remove = true;
// If we have three points in single line, we can remove the
// middle one
for (int point1 = 1; point1 < c->points.size(); ++point1) {
int point0 = point1 - 1;
int point2 = point1 + 1;
if (point2 == c->points.size()) {
break;
}
// if (p1.x == p2.x == p3.x) or (p1.y == p2.y == p3.y)
if ((NORM(c->points[point0].x()) == NORM(c->points[point1].x()) &&
NORM(c->points[point2].x()) == NORM(c->points[point1].x())) ||
(NORM(c->points[point0].y()) == NORM(c->points[point1].y()) &&
NORM(c->points[point2].y()) == NORM(c->points[point1].y()))) {
removedSome = true;
qDebug() << "removing duplicate" << point1;
removePoint(c, point1);
point1--;
}
}
// If we have two points on the same field in grid, we can remove
// the second one.
while(remove) {
remove = false;
for (int point1 = 0; point1 < c->points.size(); ++point1) {
QRect r1(c->points[point1].x() - 6, c->points[point1].y() - 6, 12, 12);
int point2 = point1 + 1;
for (; point2 < c->points.size(); ++point2) {
QRect r2(c->points[point2].x() - 6, c->points[point2].y() - 6, 12, 12);
if (r1.intersects(r2)) {
// remove = true;
break;
}
}
if (remove) {
for (int i = point1; i < point2; i++) {
removePoint(c, point1);
removedSome = true;
}
break;
}
}
}
if (removedSome) {
m_screen->repaint();
}
}
void CreatorWrapper::pointCmdCallback(const graph_planner::PointCmd::ConstPtr& cmd) {
std::cerr << "Got new cmd for point" << std::endl;
graphPath_ = nullptr;
// add the point to the graph
if (cmd->cmd == PointCMD::new_point) {
std::cerr << "Add new Point" << std::endl;
int32_t point_key_id = addPoint(
cmd->pointInfo.x,
cmd->pointInfo.y,
cmd->pointInfo.name);
//interactive marker
addInteractiveMarker(point_key_id);
}
// remove selected point
if (cmd->cmd == PointCMD::del_point) { // change cmd value
std::cerr << "Remove selected Point" << std::endl;
removePoint(cmd->pointInfo.key_id);
marker_server_->erase(std::to_string(cmd->pointInfo.key_id));
marker_server_->applyChanges();
}
pubGraph();
pubRvizGraph();
}
void
NurbsGeometry::remove(Vertex& vertex)
{
removePoint(vertex);
_vertexNameMap.erase(vertex._id);
_representation.remove(vertex);
}
void Model::removeSegment(SegmentModel* m)
{
m_segments.remove(m->id());
segmentRemoved(m->id());
for (PointModel* pt : m_points)
{
if (pt->previous() == m->id())
{
pt->setPrevious(OptionalId<SegmentModel>{});
}
if (pt->following() == m->id())
{
pt->setFollowing(OptionalId<SegmentModel>{});
}
if (!pt->previous() && !pt->following())
{
removePoint(pt);
}
}
delete m;
}
void CCurveScene::keyPressEvent(QKeyEvent *event)
{
if (((event->key() == Qt::Key_Backspace) ||
(event->key() == Qt::Key_Delete)) && (_selectedItem >= 0)) {
// user hit delete with cv selected
removePoint(_selectedItem);
}
}
void CCurveScene::contextMenuEvent(QGraphicsSceneContextMenuEvent* event){
if(_selectedItem>=0){
QMenu *menu = new QMenu(event->widget());
QAction *deleteAction = menu->addAction("Delete Point");
QAction *action = menu->exec(event->screenPos());
if (action == deleteAction) removePoint(_selectedItem);
}
}
void polyEditable::keyPressed(ofKeyEventArgs& event)
{
if( !bEnabled || !bUseKeyPress ) return;
if( event.key == ' ') nextMode();
// Note:; may not work cross-platform, tab through points
if(event.key == 9 )
{
if( mode == POLY_EDIT_MODE_MOVE_PTS )
{
selectedPoint++;
selectedPoint %= (int)(pts.size());
}
}
cout << "key " << event.key << " " << OF_KEY_BACKSPACE << endl;
if( event.key == OF_KEY_BACKSPACE ) removePoint();
if( event.key == OF_KEY_UP )
{
ofxVec2f mup = ofxVec2f(0,-moveBy);
mup.rotate(-gRotation);
if( mode == POLY_EDIT_MODE_MOVE_ALL ) moveAllPointsBy( ofPoint(mup.x,mup.y) );//ofPoint(0,-1) );
else if( mode == POLY_EDIT_MODE_MOVE_PTS ) movePointBy( selectedPoint, ofPoint(mup.x,mup.y) );//ofPoint(0,-1) );
}
else if( event.key == OF_KEY_DOWN)
{
ofxVec2f mdwn = ofxVec2f(0,moveBy);
mdwn.rotate(-gRotation);
if( mode == POLY_EDIT_MODE_MOVE_ALL ) moveAllPointsBy( ofPoint(mdwn.x,mdwn.y) );
else if( mode == POLY_EDIT_MODE_MOVE_PTS ) movePointBy( selectedPoint, ofPoint(mdwn.x,mdwn.y) );
}
else if( event.key == OF_KEY_LEFT )
{
ofxVec2f m = ofxVec2f(-moveBy,0);
m.rotate(-gRotation);
if( mode == POLY_EDIT_MODE_MOVE_ALL ) moveAllPointsBy( ofPoint(m.x,m.y) );
else if( mode == POLY_EDIT_MODE_MOVE_PTS ) movePointBy( selectedPoint, ofPoint(m.x,m.y) );
else if( mode == POLY_EDIT_MODE_ROTATE ) rotate(-.5);
}
else if( event.key == OF_KEY_RIGHT )
{
ofxVec2f m = ofxVec2f(moveBy,0);
m.rotate(-gRotation);
if( mode == POLY_EDIT_MODE_MOVE_ALL ) moveAllPointsBy( ofPoint(m.x,m.y) );
else if( mode == POLY_EDIT_MODE_MOVE_PTS ) movePointBy( selectedPoint, ofPoint(m.x,m.y));
else if( mode == POLY_EDIT_MODE_ROTATE ) rotate(.5);
}
}
void NurbsGeometry::remove(Curve& curve)
{
_representation.remove(curve);
Curve::ControlPointIteratorType iter = curve.getControlPoints();
Curve::ControlPointIteratorType end = curve.getLastControlPoint();
for ( ; iter != end; iter++) {
removePoint(*(*iter));
}
}
void
NurbsGeometry::remove(Surface& surface)
{
_representation.remove(surface);
Surface::ControlPointIteratorType iter = surface.getControlPoints();
Surface::ControlPointIteratorType end = surface.getLastControlPoint();
for ( ; iter != end; iter++) {
removePoint(*(*iter));
}
}
/**
* Takes away the last point entered by the user.
* @return true if a point was removed and false if no points exist
*/
bool removeLastPoint(void) {
if (numControlPoints <= 0) {
cout << "No point to remove." << endl;
return false;
} // if
if (numControlPoints <= 5) {
draw3D = false;
} // if
return removePoint(numControlPoints);
} // removeLastPoint
bool MgBaseLines::_setHandlePoint(UInt32 index, const Point2d& pt, float tol)
{
Int32 preindex = (_closed && 0 == index) ? _count - 1 : index - 1;
UInt32 postindex = (_closed && index + 1 == _count) ? 0 : index + 1;
float predist = preindex < 0 ? _FLT_MAX : getPoint(preindex).distanceTo(pt);
float postdist = postindex >= _count ? _FLT_MAX : getPoint(postindex).distanceTo(pt);
if (predist < tol || postdist < tol) {
removePoint(index);
}
else if (!_closed && ((index == 0 && getPoint(_count - 1).distanceTo(pt) < tol)
|| (index == _count - 1 && getPoint(0).distanceTo(pt) < tol))) {
removePoint(index);
_closed = true;
}
else {
setPoint(index, pt);
}
update();
return true;
}
void ossimGui::RegistrationOverlay::addPoint(const ossimDpt& scenePt, const ossimDpt& imagePt, const ossimString& id)
{
if (id != "NS")
{
// Check for duplicate (changing position of point already added)
removePoint(id);
// Add point to scene
ossimGui::RegPoint* pt = new ossimGui::RegPoint(scenePt, imagePt, m_overlayId, id);
m_scene->addItem(pt);
// Notify MultiImageDialog
emit pointActivated(id);
}
}
void myMouseButton(int button, int state, int x, int y)
{
GLfloat px, py, pz;
if (state == GLUT_DOWN)
{
if(!mode) /* 2D mode */
{
if (button == GLUT_LEFT_BUTTON)
{
if((W-x-10)>=(W/2)) x = W/2;
printf("W-x-10 == %d\n",W-x-5);
px = dpleft + (x*fabs(dpleft-dpright))/W;
/* assuming that the axis of rotation is at px=0 */
if(px<0) px = 0;
py = dptop - (y*fabs(dptop-dpbottom))/H;
pz = 0;
addPoint(px,py,pz);
//printf("x: %3d, y: %3d, i: %3d\n",x,y,num++);
//printf("px: %3f, py: %3f, pz: %3f\n",px,py,pz);
num++;
}
else if(button==GLUT_RIGHT_BUTTON)
{
if(num>0)
{
printf("removing control point %d\n",--num);
removePoint();
}
}
glutPostRedisplay();
}
else /* 3D mode */
{
m_last_x = x;
m_last_y = y;
if (button == GLUT_LEFT_BUTTON)
mouse_mode = MOUSE_ROTATE_YX;
else if (button == GLUT_MIDDLE_BUTTON)
mouse_mode = MOUSE_ROTATE_YZ;
else if (button == GLUT_RIGHT_BUTTON)
mouse_mode = MOUSE_ZOOM;
}
}
}
LDLFileLineArray *LDLQuadLine::removeMatchingPoint(void)
{
LDLFileLineArray *fileLineArray = removePoint(m_matchingIndex);
if (fileLineArray)
{
UCCHAR pointBuf[64] = _UC("");
printPoint(m_matchingIndex, pointBuf);
setWarning(LDLEMatchingPoints,
TCLocalStrings::get(_UC("LDLQuadLineIdentical")),
m_matchingIndex + 1, pointBuf);
}
else
{
setError(LDLEGeneral,
TCLocalStrings::get(_UC("LDLQuadLineIdenticalError")));
}
return fileLineArray;
}
LDLFileLineArray *LDLQuadLine::removeMatchingPoint(void)
{
LDLFileLineArray *fileLineArray = removePoint(m_matchingIndex);
if (fileLineArray)
{
UCSTRING pointStr;
pointStr = printPoint(m_matchingIndex);
setWarning(LDLEMatchingPoints,
TCLocalStrings::get(_UC("LDLQuadLineIdentical")),
m_matchingIndex + 1, pointStr.c_str());
}
else
{
setError(LDLEGeneral,
TCLocalStrings::get(_UC("LDLQuadLineIdenticalError")));
}
return fileLineArray;
}
char execCommand(InPackStruct* cmd) //обработать входящую команду
{
switch(cmd->command)
{
case 0x01: //Эхо
{
char *key= cmd->param;
if ((key[0] =='E')&&(key[1] =='C')&&(key[2] =='H')&&(key[3] =='O') )
{
char * str ="mobile robot V1.0";
sendAnswer(cmd->command, str, strlen(str)+1);
}
}
break;
case 0x02: //Установить текущие координаты
{
float *(temp) ={(float*)cmd->param};
robotCoord[0]= temp[0];
robotCoord[1]= temp[1];
robotCoord[2]= temp[2];
points[0].center[0]= temp[0];
points[0].center[1]= temp[1];
points[0].center[2]= temp[2];
CreatePath(&points[0], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x03: //установить скважность шим
{
char ch = *cmd->param;
float temp =*((float*)(cmd->param + 1));
setPWM( ch - 1, temp);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x04: //Установить бит направления
{
char * ch = cmd->param;
set_pin(PWM_DIR[(*ch)-1]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x05: //Снять бит направления
{
char * ch = cmd->param;
reset_pin(PWM_DIR[(*ch)-1]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x06: //Установить напряжение на двигателе
{
char ch = *cmd->param;
float duty = *((float*)(cmd->param + 1));
uint16_t dir = *((uint16_t*)(cmd->param + 2));
setVoltageMaxon( ch-1, dir, duty);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x08: //Установить параметры регулятора
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i<=3; i++)
{
wheelsPidStruct[i].p_k = temp[0];
wheelsPidStruct[i].i_k = temp[1];
wheelsPidStruct[i].d_k = temp[2];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x09: //Установить требуюему скорость двигателей
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i <= 3; i++)
{
regulatorOut[i] = temp[i];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0B: //Включить рассчет кинематики
{
curState.kinemEn=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0C: //Выключить рассчет кинематики
{
curState.kinemEn = 0;
char * str = "Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0D: //Задать скорости движения
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i<=2; i++)
{
vTargetGlob[i] = temp[i];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0E: //Включить траекторный регулятор
{
curState.trackEn=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0F: //Выключить траекторный регулятор
{
curState.trackEn=0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x10: //Очистить очередь точек
{
while(lastPoint>0) removePoint(&points[0],&lastPoint);
points[0].center[0]= robotCoord[0];
points[0].center[1]= robotCoord[1];
points[0].center[2]= robotCoord[2];
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x11: //Добавить точку в очередь
{
float *(temp) ={(float*)(cmd->param)};
char * ch = cmd->param + 12;
lastPoint++;
points[lastPoint].center[0] = temp[0];
points[lastPoint].center[1] = temp[1];
points[lastPoint].center[2] = temp[2];
points[lastPoint].speedVelTipe = speedType[*ch];
points[lastPoint].speedRotTipe = rotType[*ch];
points[lastPoint].endTask = NULL;
points[lastPoint].movTask = NULL;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x12: //Состояние очереди точек
{
char outdata[15];
float * temp =(float*)(&outdata[3]);
char * cntPoint = (&outdata[0]);
uint16_t * curPoint = (uint16_t *)(&outdata[1]);
*cntPoint= lastPoint;
*curPoint = totalPointComplite;
temp[0]= points[0].center[0];
temp[1]= points[0].center[1];
temp[2]= points[0].center[2];
//char * str ="Ok";
sendAnswer(cmd->command,outdata, 15);
}
break;
case 0x13: //отправить текущие координаты
{
sendAnswer(cmd->command,(char *)robotCoord, sizeof(robotCoord));
}
break;
case 0x14: //отправить текущую скорость
{
sendAnswer(cmd->command,(char *)robotSpeed, sizeof(robotCoord));
}
break;
case 0x15: //Задать скорость движения
{
float *(temp) = {(float*)(cmd->param)};
char i;
for (i = 0; i<=4; i++)
normalVelFast[i]= temp[i];
for (i = 0; i<=4; i++)
stopVelFast[i]= temp[i];
stopVelFast[2]=-0.2;
char * str = "Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x16: //Установить режим ножки
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
pinType[ch] = *((char *)(cmd->param +1));
if (pinType[ch] == ADC_ANALOG_PIN )
conf_pin(GENERAL_PIN[ch], ANALOG, PUSH_PULL, FAST_S, NO_PULL_UP);
if (pinType[ch] == ADC_DIG_INPUT )
conf_pin(GENERAL_PIN[ch], INPUT, PUSH_PULL, FAST_S, NO_PULL_UP);
if (pinType[ch] == ADC_DIG_OUTPUT )
conf_pin(GENERAL_PIN[ch], GENERAL, PUSH_PULL, FAST_S, NO_PULL_UP);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x17: //отправить состояние выбранного входа АЦП
{
char ch = (*((char *)(cmd->param))) - 1;
if (ch < 10)
sendAnswer(cmd->command,(char *)&(adcData[ch]), sizeof(uint16_t));
}
break;
case 0x18: //отправить состояние всех АЦП
{
sendAnswer(cmd->command,(char *)adcData, sizeof(adcData));
}
break;
case 0x19: //отправить состояние входа
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
char temp = (pin_val(GENERAL_PIN[ch])!=0);
sendAnswer(cmd->command,&temp, sizeof(temp));
}
}
break;
case 0x1A: //отправить состояние всех входов
{
char temp[10];
char i ;
for ( i = 0; i<10; i++) temp[i] = (pin_val(GENERAL_PIN[i])!=0);
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x1B: //установить состояние выхода
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
if (*(cmd->param+1)==0) reset_pin(GENERAL_PIN[ch]); else
set_pin(GENERAL_PIN[ch]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x1C: //отправить текущий режим ножки
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
sendAnswer(cmd->command,(char *) &(pinType[ch]), sizeof(uint8_t));
}
break;
case 0x1D: //установить режим ножки EXTI
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
extiType[ch] = (*((char *)(cmd->param +1)));
if (extiType[ch] == EXTI_BOTH )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_BOTH_EDGES);
}
if (extiType[ch] == EXTI_RISE )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_RISING_EDGE);
}
if (extiType[ch] == EXTI_FALL )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_FALLING_EDGE) ;
}
if (extiType[ch] == EXTI_DIG_INPUT )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, NO_PULL_UP);
clear_ext_interrupt(EXTI_PIN[ch]) ;
}
if (extiType[ch] == EXTI_DIG_OUTPUT )
{
conf_pin(EXTI_PIN[ch], GENERAL, PUSH_PULL, FAST_S, NO_PULL_UP);
clear_ext_interrupt(EXTI_PIN[ch]) ;
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x1E: //отправить состояние входа
{
char ch = *((char *)(cmd->param))-1;
if ((ch)<10)
{
char temp = ch;
if (pin_val(EXTI_PIN[ch])) temp |=0x80;
sendAnswer(cmd->command,&temp, sizeof(temp));
}
}
break;
case 0x1F: //отправить состояние всех входов
{
char temp[10];
char i ;
for ( i = 0; i<10; i++) temp[i] = (pin_val(EXTI_PIN[i])!=0);
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x20: //установить состояние выхода
{
char ch = *((char *)(cmd->param))-1;
if (ch<10)
if (*(cmd->param+1)==0) reset_pin(EXTI_PIN[ch]); else
set_pin(EXTI_PIN[ch]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x21: //отправить текущий режим ножки
{
char ch = *((char *)(cmd->param))-1;
if (ch<10)
sendAnswer(cmd->command,(char *) &(extiType[ch]), sizeof(uint8_t));
}
break;
case 0x22: //установить состояние выхода +12В
{
char ch = (*((char *)(cmd->param)))-1;
if (ch<6)
{
if (*(cmd->param+1)==0)
reset_pin(V12_PIN[ch]); else
set_pin(V12_PIN[ch]);
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x23: //Выключить ПИД регуляторы приводов
{
curState.pidEnabled=0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x24: //Включить ПИД регуляторы приводов
{
curState.pidEnabled=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x25: //set current coordinate
{
float *(temp) ={(float*)cmd->param};
robotCoord[0]= temp[0];
robotCoord[1]= temp[1];
robotCoord[2]= temp[2];
addPointInFrontOfQueue(&points[0], &temp[0], (char) 4, &lastPoint);
CreatePath(&points[1], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
case 0x26: //set dynamixel angle
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(angle) ={(uint16_t*)(cmd->param + 1)};
if (setServoMovingSpeed(ID, angle))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x27: //set CW angle limit
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(limit) ={(uint16_t*)(cmd->param + 1)};
if (setServoCWAngleLimit(ID, limit))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x28: //set CCW angle limit
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(limit) ={(uint16_t*)(cmd->param + 1)};
if (setServoCCWAngleLimit(ID, limit))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x29: //set servo moving speed
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(speed) ={(uint16_t*)(cmd->param + 1)};
uint16_t *(direction) ={(uint16_t*)(cmd->param + 3)};
if (setServoMovingSpeed(ID, speed, direction))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x2A: //add a point to the beginning of Queue
{
float *(temp) = (float*)(cmd->param);
char * ch = cmd->param + 12;
addPointInFrontOfQueue(&points[0], &temp[0], &ch, &lastPoint);
CreatePath(&points[1], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x2B: //Open Cubes Catcher
{
openCubesCatcher();
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x2C: //Close Cubes Catcher
{
uint8_t numberOfCubesCatched;
closeCubesCatcher(&numberOfCubesCatched);
sendAnswer(cmd->command, (char *)&numberOfCubesCatched, sizeof(uint8_t));
}
break;
case 0x2D: // Open wall
{
openWall();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x2E: // Close wall
{
closeWall();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x2F: // Switch On the vibration
{
uint8_t temp ={*(uint8_t*)(cmd->param)};
switchOnVibration(temp);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x30: // Switch Off the vibration
{
switchOffVibration();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x31: // Set angle of cubes catcher
{
float *(temp) = (float*)(cmd->param);
cubesCatcherPID.target = *temp;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x32: // Flag of reached point
{
sendAnswer(cmd->command, (char *)&traceFlag, sizeof(traceFlag));
}
break;
case 0x33: // Switch on collision avoidance
{
curState.collisionAvoidance = 1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x34: // Switch off collision avoidance
{
curState.collisionAvoidance = 0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x35: // Get Cubes Catcher angle
{
sendAnswer(cmd->command, (char *)&cubesCatcherPID.current, sizeof(cubesCatcherPID.current));
}
break;
case 0x36: // Get IR Distances
{
float temp[4];
int i = 0;
for(; i < 5; i++)
{
temp[i] = distanceFromIR[i][0];
}
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x37: // Get Sonar Distances
{
float temp[5];
int i = 0;
for(; i < 6; i++)
{
temp[i] = distanceFromSonars[i][0];
}
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x38: // Stop command
{
closeWall();
openCubesCatcher();
curState.pidEnabled = 0;
switchOffVibration();
char i;
for (i = 0; i < 4; i++)
{
setVoltageMaxon(WHEELS[i], (uint8_t) 1, (float) 0);
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x39: // Generate new trajectory with correction
{
float *(temp) ={(float*)cmd->param};
char * ch = cmd->param + 24;
robotCoord[0] = temp[0];
robotCoord[1] = temp[1];
robotCoord[2] = temp[2];
lastPoint++;
points[lastPoint].center[0] = temp[3];
points[lastPoint].center[1] = temp[4];
points[lastPoint].center[2] = temp[5];
points[lastPoint].speedVelTipe = speedType[*ch];
points[lastPoint].speedRotTipe = rotType[*ch];
points[lastPoint].endTask = NULL;
points[lastPoint].movTask = NULL;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3A: // Open cubes catcher widely
{
openCubesCatcherWidely();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3B : // Kick the cone on a purple side
{
moveCone();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3C: // Close the cone kicker cone on a purple side
{
closeCone();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
default:
break;
}
return 0;
}
void servoControl(){
//printf("BeaconVisible=%d beaconDir=%d left=%d right=%d ServoPos=%d\n", beaconVisible, beaconDir, left, right, currServoPos);
//led(3,visible);
if(followPoints){
//printf("FOLLOW POINT\n");
getLastPoint();
targetX=lastPointX;
targetY=lastPointY;
if( checkPointsRadius() ){
if(removePoint()){
// update point
printf("Found home!\n");
setVel2(0,0);
while (!startButton());
}
}
//double myNorm, targetNorm;
double meToTargetVector [2];
double myDirectionVector [2];
double myX=x, myY=y, myDir=t; // 0.35 radians
meToTargetVector[0]=(targetX-myX);
meToTargetVector[1]=(targetY-myY);
myDirectionVector[0]=cos(myDir);
myDirectionVector[1]=sin(myDir);
double radianBeaconDir = atan2(meToTargetVector[1], meToTargetVector[0]) - atan2(myDirectionVector[1], myDirectionVector[0]);
beaconDir=-normalizeAngle(radianBeaconDir)*(180 / PI);
int servoPos = beaconDir/6;
printf("My position %2.1f,%2.1f; Target Position %d,%d; MyAngle %2.0f AngleToNextPoint %d\n",
myX, myY, targetX, targetY, (myDir*180/PI), beaconDir); //NEGATIVO PRA ESQUERDA
if(servoPos > POS_RIGHT)
setServoPos(POS_RIGHT);
else if( servoPos < POS_LEFT )
setServoPos(POS_LEFT);
else
setServoPos(servoPos);
}else{
bVis=readBeaconSens();
if(bVis && !oldBVis){ // Started seeing beacon!
if(rotate_right){
left = currServoPos;
//if(left<0 && right>=POS_RIGHT)
// right=left+15;
}
else {
right = currServoPos;
//if(right>0 && left<=POS_LEFT)
// left=right-15;
}
if(currServoPos==POS_RIGHT || currServoPos==POS_LEFT){ // Got to the end of the line
rotate_right=!rotate_right;
}
}
else if(!bVis && oldBVis){ // Stopped seeing beacon!
if(rotate_right){ right = currServoPos-1; }
else { left = currServoPos+1; }
rotate_right=!rotate_right;
} else if(currServoPos==POS_RIGHT || currServoPos==POS_LEFT){ // Got to the end of the line
if(currServoPos==POS_RIGHT){ right = POS_RIGHT+1; }
else { left = POS_LEFT-1; }
rotate_right=!rotate_right;
}
visible = bVis||oldBVis||oldBVis2;
//led(2, visible);
oldBVis2=oldBVis;
oldBVis=bVis;
beaconDir = (left + right)/2*6;
//printf("Beacon at %d\n",beaconDir);
modder = rotate_right ? 1 : -1;
//currServoPos+=modder;
currServoPos = visible ? currServoPos+modder*1 : currServoPos+modder*2;
if(currServoPos>POS_RIGHT){ currServoPos=POS_RIGHT; }
else if(currServoPos<POS_LEFT){ currServoPos=POS_LEFT; }
setServoPos(currServoPos);
}
}
void QgsRubberBand::removeLastPoint( int geometryIndex, bool doUpdate/* = true*/ )
{
removePoint( -1, doUpdate, geometryIndex );
}
bool Surface::collapse(Edge& e)
{
timespec t0,t1,t;
clock_gettime(CLOCK_REALTIME,&t0);
Point* p1 = e.p1;
Point* p2 = e.p2;
assert(p1);
assert(p2);
if(p1->faces.size() ==0 || p2->faces.size()==0) //Do not simplify boundary corners
{
//cerr << "*ERROR: EMPTY VERTEX.\n";
failed_collapses++;
//cout << redtty << "failed.\n" << deftty;
return false;
}
//Iterating faces
vector<Face*> for_removal;
//clock_gettime(CLOCK_REALTIME,&t0);
for(vector<Face*>::iterator fit = p1->faces.begin(); fit!=p1->faces.end(); ++fit)
{
bool removeface = false;
vector<Point*>::iterator auxp1;
for(vector<Point*>::iterator pit = (*fit)->points.begin(); pit != (*fit)->points.end() ; ++pit)
{
if((*pit) == p2)
{
removeface = true;
break;
}
else if ((*pit) == p1)
{
auxp1 = pit;
}
}
if(removeface) //p1 and p2 share face, remove it.
{
for_removal.push_back((*fit));
}
else //Swap p1 for p2 for this face
{
//Face is about to be modified. Checking intersection.
Point3 p30((*fit)->points[0]->x,(*fit)->points[0]->y,(*fit)->points[0]->z);
Point3 p31((*fit)->points[1]->x,(*fit)->points[1]->y,(*fit)->points[1]->z);
Point3 p32((*fit)->points[2]->x,(*fit)->points[2]->y,(*fit)->points[2]->z);
Triangle3 face(p30,p31,p32);
for(face_vec_it fit2 = m_faces.begin(); fit2 != m_faces.end(); ++fit2)
{
Face* f = (*fit2);
Point3 pf0(f->points[0]->x,f->points[0]->y,f->points[0]->z);
Point3 pf1(f->points[1]->x,f->points[1]->y,f->points[1]->z);
Point3 pf2(f->points[2]->x,f->points[2]->y,f->points[2]->z);
Triangle3 face2(pf0,pf1,pf2);
if(CGAL::do_intersect(face,face2))
{
cerr << "***Faces " << (*fit)->id << " X " << f->id << endl;
}
}
(*auxp1) = p2;
p2->faces.push_back((*fit));
}
}
//cerr << "Removing faces: ";
for(vector<Face*>::iterator it = for_removal.begin(); it != for_removal.end(); ++it)
{
//cerr << (*it)->id << " ";
removeFace(*it);
(*it)->removed = true;
//delete (*it);
}
//Set position of p2 as midpoint between p1-p2
p2->x = e.placement->x;
p2->y = e.placement->y;
p2->z = e.placement->z;
clock_gettime(CLOCK_REALTIME,&t1);
t = diff(t0,t1);
time_faces+= getNanoseconds(t);
//TODO: more efficient to use std::remove_if
clock_gettime(CLOCK_REALTIME,&t0);
removeEdge(m_edges[e.id]);
vector<Edge*> edges_to_remove;
edge_vec_it eit = p1->to.begin();
//Remove double edges to
while(eit != p1->to.end())
{
Edge* e = (*eit);
bool found = false;
edge_vec_it it = p2->to.begin();
while(it != p2->to.end())
{
Edge* e2 = (*it);
if(e->p1->id == e2->p1->id)
{
//cerr << "Found " << e->id << " " << e->p1->id << " " << e->p2->id << endl;
edges_to_remove.push_back(e);
found = true;
break;
}
it++;
}
if(!found)
{
e->p2 = p2;
if(e->p1->id == e->p2->id)
edges_to_remove.push_back(e);
}
eit++;
}
//Remove double edges from
eit = p1->from.begin();
while(eit!=p1->from.end())
{
Edge* e = (*eit);
bool found = false;
edge_vec_it it = p2->from.begin();
while(it != p2->from.end())
{
Edge* e2 = (*it);
if(e->p2->id == e2->p2->id)
{
//cerr << "Found from " << e->id << " " << e->p1->id << " " <<e->p2->id << endl;
edges_to_remove.push_back(e);
found =true;
break;
}
it++;
}
if(!found)
{
e->p1=p2;
if(e->p1->id == e->p2->id)
edges_to_remove.push_back(e);
}
eit++;
}
eit = edges_to_remove.begin();
while(eit != edges_to_remove.end())
{
removeEdge(*eit);
eit++;
}
//Append p1 edges to p2
p2->to.insert(p2->to.end(), p1->to.begin(), p1->to.end());
p2->from.insert(p2->from.end(),p1->from.begin(), p1->from.end());
clock_gettime(CLOCK_REALTIME,&t1);
t = diff(t0,t1);
time_edges += getNanoseconds(t);
//Can remove point p1
clock_gettime(CLOCK_REALTIME,&t0);
removePoint(p1);
clock_gettime(CLOCK_REALTIME,&t1);
t = diff(t0,t1);
time_point+=getNanoseconds(t);
return true;
}
// Remove point from colourscale
void ColourScale::removePoint(ColourScalePoint* point)
{
int position = points_.indexOf(point);
if (position != -1) removePoint(position);
}
bool ConnectionManager::mousePressEvent(QMouseEvent *event) {
if (event->button() == Qt::RightButton) {
if (m_fromPin != -1) {
m_fromPin = -1;
m_points.clear();
return true;
}
int point;
Connection *c = getPoint(NORM(event->x()), NORM(event->y()), point);
if (point != -1) {
// User can't remove first or last point like that
if (point == 0 || point == c->points.size() - 1) {
return false;
}
QList<QAction *> actions;
actions.append(new QAction("Remove point", 0));
QAction *action = QMenu::exec(actions, event->globalPos(), 0, 0);
if (action) {
removePoint(c, point);
}
return true;
}
c = getConnection(NORM(event->x()), NORM(event->y()), point);
if (c) {
QList<QAction *> actions;
actions.append(new QAction("Remove connection", 0));
QAction *action = QMenu::exec(actions, event->globalPos(), 0, 0);
if (action) {
removeConnection(c);
}
return true;
}
}
else if (event->button() == Qt::LeftButton) {
if (m_fromPin != -1) {
QPoint from = QPoint(m_movingX, m_movingY);
QPoint to(NORM(event->pos().x()), NORM(event->pos().y()));
ScreenObject *object = m_screen->getObject(NORM(event->x()), NORM(event->y()));
if (object) {
int pin = m_screen->getPin(object, NORM(event->x()), NORM(event->y()));
if (pin != -1) {
to = object->getPins()[pin].rect.center();
}
}
bool firstPoints = m_points.empty();
if (firstPoints) {
m_points.push_back(from);
}
if (m_screen->getObject(from.x(), to.y()) == m_fromObject/* &&
m_screen->getPin(m_moving, from.x(), to.y()) != -1*/) {
m_points.push_back(QPoint(to.x(), from.y()));
}
else {
m_points.push_back(QPoint(from.x(), to.y()));
}
// if (!firstPoints) {
m_points.push_back(to);
// }
int point;
Connection *c = getPoint(NORM(event->x()), NORM(event->y()), point);
if (point != -1) {
ScreenObject *object = m_screen->getObject(NORM(event->x()), NORM(event->y()));
if (object) {
m_points.erase(m_points.end() - 1);
m_points.erase(m_points.end() - 1);
return false;
}
addConnectionNode(c, point, event->x(), event->y(), m_points);
m_fromPin = -1;
m_points.clear();
// qDebug() << newPoints.size() << c->points.size() << "\n";
return true;
}
else {
QPointF intersectPnt;
int point;
c = getConnection(NORM(event->x()), NORM(event->y()), point, &intersectPnt);
if (c) {
QPoint p = intersectPnt.toPoint();
p = QPoint(NORM(p.x()), NORM(p.y()));
c->points.insert(c->points.begin() + point, p);
addConnectionNode(c, point, event->x(), event->y(), m_points);
m_fromPin = -1;
m_points.clear();
return true;
}
}
if (object) {
int pin = m_screen->getPin(object, NORM(event->x()), NORM(event->y()));
if (pin != -1) {
addConnection(m_fromObject, m_fromPin, object, pin, m_points);
m_fromPin = -1;
m_points.clear();
return true;
}
}
m_movingX = NORM(event->x());
m_movingY = NORM(event->y());
return true;
}
// qDebug() << NORM(event->x()) << NORM(event->y());
int point;
Connection *c = getPoint(NORM(event->x()), NORM(event->y()), point);
if (point != -1) {
if (point == 0) {
m_fromObject = c->from;
}
else if (point == c->points.size() - 1) {
m_fromObject = c->to;
}
else {
m_fromObject = 0;
}
m_movingConn = c;
m_moving = &c->points[point];
m_movingX = NORM(event->x());
m_movingY = NORM(event->y());
return true;
}
ScreenObject *object = m_screen->getObject(NORM(event->x()), NORM(event->y()));
if (!object) {
return false;
}
m_fromPin = m_screen->getPin(object, NORM(event->x()), NORM(event->y()));
if (m_fromPin != -1) {
m_fromObject = object;
m_movingX = object->getPins()[m_fromPin].rect.center().x();
m_movingY = object->getPins()[m_fromPin].rect.center().y();
return true;
}
}
return false;
}
void GraphChart::randDirection(){
int limit = 1000000;
int lottery = rand() % limit;
if(lottery > limit - m_chartPoints.size()*10000 ){
int number = rand() % m_chartPoints.size();
_SharedPtr<GraphChartPoint> point1 = m_chartPoints.at(number);
if(point1){
int direction = rand() % 9 + 1;
switch(direction){
case 1:
removePoint(point1);
if(point1->m_X > 0)
point1->m_X--;
if(point1->m_Y < m_rows-1)
point1->m_Y++;
break;
case 2:
removePoint(point1);
if(point1->m_Y < m_rows-1)
point1->m_Y++;
break;
case 3:
removePoint(point1);
if(point1->m_X < m_cols)
point1->m_X++;
if(point1->m_Y < m_rows-1)
point1->m_Y++;
break;
case 4:
removePoint(point1);
if(point1->m_X > 0)
point1->m_X--;
break;
case 5:
break;
case 6:
removePoint(point1);
if(point1->m_X < m_cols)
point1->m_X++;
break;
case 7:
removePoint(point1);
if(point1->m_X > 0)
point1->m_X--;
if(point1->m_Y > 0)
point1->m_Y--;
break;
case 8:
removePoint(point1);
if(point1->m_Y > 0)
point1->m_Y--;
break;
case 9:
removePoint(point1);
if(point1->m_X < m_cols)
point1->m_X++;
if(point1->m_Y > 0)
point1->m_Y--;
break;
}
}
}
}
