void TestLocRotScaleLevel2(Transform& gparent, Transform& parent, Transform& child) { Transform p = parent; Transform gp = gparent; // Location child.set_local_pos(RandomVec()); CheckPos(parent, child, "Translation effect on child"); AssertEquals(parent, p, "Child's translation has no effect parent"); // Rotation parent.set_rot(RandomQuat()); CheckPos(parent, child, "Rotation effect on child"); AssertEquals(parent.pos(), p.pos(), "Rotation invariant on local_pos"); p = parent; gp = gparent; child.set_rot(parent.rot()); AssertEquals(parent, p, "Child's rotation has no effect on parent"); AssertEquals(gparent, gp, "Child's rotation has no effect on gparent"); // Scale parent.set_local_scale(RandomVec()); CheckPos(parent, child, "Scale effect on child"); AssertEquals(parent.pos(), p.pos(), "Scale invariant on local_pos"); p = parent; gp = gparent; child.set_scale(parent.scale()); AssertEquals(parent, p, "Child's scaling has no effect parent"); AssertEquals(gparent, gp, "Child's scaling has no effect on gparent"); }
bool Bishop::CheckMove(int startX, int startY, int destX, int destY, const Square Board[][boardsize]) { if(!MovesOnEnemy(destX,destY,startunit(startY,startX)->isWhite,Board)){ return false; } int x(startX), y(startY); //bot left if(startX > destX && startY > destY){ while(x < destX && y < destY){ if(CheckPos(--x,--y,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } //top left if(startX > destX && startY < destY){ while(x < destX && y < destY){ if(CheckPos(--x,++y,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } //bot right if(startX < destX && startY > destY){ while(x < destX && y < destY){ if(CheckPos(++x,--y,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } //top right if(startX < destX && startY < destY){ while(x < destX && y < destY){ if(CheckPos(++x,++y,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } return false; }
void SingleCommandCallback(const SpiderRobot_pkg::My2Num::ConstPtr& msg) { char buffer[20] = {'\0'}, temp[10] = {'\0'}; // buffer for serial commands, and temp buffer int i, n, uSecPos, result; //~ printf("\nRecived single channel data...\n"); uSecPos = (int)(11.3333* ((float)msg->pos) + 1500); // convert to useconds of duty cycle CheckPos(1, NULL, msg->cha, &uSecPos); // check to make sure desired position is in range of possible position, mode 1 for only 1 channel sprintf(temp, "#%dP%d",msg->cha, uSecPos); // convert to string for first motor (0) strcat(buffer, temp); strcat(buffer, "S250"); // set slow speed n = strlen(buffer); //~ printf("size: %d\n", n); //~ for(i = 0 ; i < n ; i++) // print full command for checking //~ printf("%c", buffer[i]); //~ printf("\n"); buffer[n] = 13; // ascii carrier return, used for end bit. Should also remove null result = write(serialPort, buffer, n+1); // send to ssc-32 //~ printf("bits sent: %d\n", result); // print bits sent }// end SingleCommandCallback
/**\brief Scroll to position on mouse down. */ bool Scrollbar::MouseLDown( int x, int y ){ Widget::MouseLDown( x, y ); // Relative coordinate - to current widget int xr = x - GetX(); int yr = y - GetY(); int newpos; if( yr < bitmaps[1]->GetHeight() ) { // Click on the UP arrow newpos = 0; //pos - SCROLLBAR_SCROLL; } else if ( yr > (h - bitmaps[2]->GetHeight()) ) { // Click on the Down arrow newpos = maxpos; // pos + SCROLLBAR_SCROLL; } else { // Click somewhere in the middle newpos = MarkerPixelToPos( xr, yr ); } pos = CheckPos( newpos ); return true; }
//void motionCommandCallback(const std_msgs::Int16MultiArray::ConstPtr& msg) void motionCommandCallback(const SpiderRobot_pkg::MyArray::ConstPtr& msg) { char bufferR[300]; switch(msg->command) { case 0: // Update all joint commands via speed { // command case 0: send command to servos, most common case // char bufferR[300]; char temp[10] = {'\0'}; // buffer for serial commands, and temp buffer int i, n, pos[18], uSecPos[18], result; // counter i, size n, position commands in degree, position commands in usec, result from sending serial data printf("\nRecived data for all channels...\n"); for(i=0 ; i<18; i++) // get position values { pos[i] = msg->data[i]; } for(i=0 ; i<18; i++) { uSecPos[i] = (int)(11.3333* ((float)pos[i]) + 1500.0); // convert to useconds of duty cycle } // check to make sure desired position is in range of possible position, mode 0 for all channels CheckPos(0, uSecPos, NULL, NULL); sprintf(temp, "#0P%d", uSecPos[0]); // convert to string for first motor (0) strcat(bufferR, temp); for(i=1 ; i<18; i++) { if(i < 9) // motor number, channel offset { sprintf(temp, "#%dP%d", i, uSecPos[i]); // convert to string for the rest of the motors (i) with command uSecPos } else if(8 < i && i < 13) { sprintf(temp, "#%dP%d", i+7, uSecPos[i]); // +7 is the channel motor offset } else //(12 < i && i < 29) { sprintf(temp, "#%dP%d", i+11, uSecPos[i]); // +11 is the channel motor offset } strcat(bufferR, temp); // keep appending commands memset(temp, '\0', sizeof(temp)); // clear temp } if(msg->speed > 0 && 1000 > msg->speed) { sprintf(temp, "S%d", msg->speed); // add speed to array if it exists } else { sprintf(temp, "S250"); // default if it doesnt exist } strcat(bufferR, temp); n = strlen(bufferR); printf("size: %d\n", n); for(i = 0 ; i < n ; i++) // print full command for checking printf("%c", bufferR[i]); printf("\n"); bufferR[n] = 13; // ascii carrier return, used for end bit. Should also remove null result = write(serialPort, bufferR, n+1); // send to ssc-32 printf("bits sent: %d\n", result); // print bits sent break; } case 1: // Update all joint commands via time { // char bufferR[300]; char temp[10] = {'\0'}; // buffer for serial commands, and temp buffer int i, n, pos[18], uSecPos[18], result; // counter i, size n, position commands in degree, position commands in usec, result from sending serial data //~ printf("\nRecived data for all channels...\n"); for(i=0 ; i<18; i++) // get position values { pos[i] = msg->data[i]; } for(i=0 ; i<18; i++) { uSecPos[i] = (int)(11.3333* ((float)pos[i]) + 1500.0); // convert to useconds of duty cycle } // check to make sure desired position is in range of possible position, mode 0 for all channels CheckPos(0, uSecPos, NULL, NULL); sprintf(temp, "#0P%d", uSecPos[0]); // convert to string for first motor (0) strcat(bufferR, temp); for(i=1 ; i<18; i++) { if(i < 9) // motor number, channel offset { sprintf(temp, "#%dP%d", i, uSecPos[i]); // convert to string for the rest of the motors (i) with command uSecPos } else if(8 < i && i < 13) { sprintf(temp, "#%dP%d", i+7, uSecPos[i]); // +7 is the channel motor offset } else //(12 < i && i < 29) { sprintf(temp, "#%dP%d", i+11, uSecPos[i]); // +11 is the channel motor offset } strcat(bufferR, temp); // keep appending commands memset(temp, '\0', sizeof(temp)); // clear temp } if(msg->speed > 5.0 && 50000 > msg->speed) { sprintf(temp, "T%d", msg->speed); // add time to array if it exists (mili sec) } else { sprintf(temp, "T1000"); // default if it doesnt exist } strcat(bufferR, temp); n = strlen(bufferR); //~ printf("size: %d\n", n); //~ for(i = 0 ; i < n ; i++) // print full command for checking //~ printf("%c", bufferR[i]); //~ printf("\n"); bufferR[n] = 13; // ascii carrier return, used for end bit. Should also remove null result = write(serialPort, bufferR, n+1); // send to ssc-32 //~ printf("bits sent: %d\n", result); // print bits sent break; } case 9: { printf("Emergency Stop Mode!!! \n Not yet functional, requires additional hardware to shutdown motors. Currently just shuts down the node\n"); SHUTDOWN = true; break; break; } } LAST_COMMAND_TS = ros::Time::now(); }// end motionCommandCallback
bool Queen::CheckMove(int startX, int startY, int destX, int destY, const Square Board[][boardsize]) { if(!MovesOnEnemy(destX,destY,startunit(startY,startX)->isWhite,Board)){ return false; } int y(startY+1); //left'n'right if(destX == startX){ if(startY < destY){ while(y < destY){ if(!CheckPos(startX,y++,Board)){ return false; } } }else{ // > while(y < destY){ if(!CheckPos(startX,y--,Board)){ return false; } } } return true; } int x(startX+1); // up'n'down if(startY == destY){ if(startX < destX){ while(x < destX){ if(!CheckPos(x++,startY,Board)){ return false; } } }else{ // > while(x < destX){ if(!CheckPos(x--,startY,Board)){ return false; } } } return true; } //Diagonals x = startX+1; y = startY+1; //bot left if(startX > destX && startY > destY){ while(x < destX && y < destY){ if(CheckPos(x--,y--,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } //top left if(startX > destX && startY < destY){ while(x < destX && y < destY){ if(CheckPos(x--,y++,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } //bot right if(startX < destX && startY > destY){ while(x < destX && y < destY){ if(CheckPos(x++,y--,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } //top right if(startX < destX && startY < destY){ while(x < destX && y < destY){ if(CheckPos(x++,y++,Board)){ return false; } } if(x == destX && y == destY){ return true; }else{ return false;} } return false; }