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;
}
