void ControlNode::setTwist(double lx, double ly, double lz,double ax,
double ay, double az)
{
setLinearX(lx);
setLinearY(ly);
setLinearZ(lz);
setAngularX(ax);
setAngularY(ay);
setAngularZ(az);
}
void Irobot::goRobotGo()
{
setLinearX(0.5);
setAngularZ(0.0);
sendCommand();
usleep(1000000);
setLinearX(0);
setAngularZ(2.3);
sendCommand();
usleep(1000000);
setLinearX(0.5);
setAngularZ(0.0);
sendCommand();
usleep(1000000);
setLinearX(0);
setAngularZ(2.3);
sendCommand();
usleep(1000000);
setLinearX(0.5);
setAngularZ(0.0);
sendCommand();
usleep(1000000);
setLinearX(0);
setAngularZ(2.3);
sendCommand();
usleep(1000000);
setLinearX(0.5);
setAngularZ(0.0);
sendCommand();
usleep(1000000);
setLinearX(0);
setAngularZ(2.3);
sendCommand();
usleep(1000000);
setLinearX(0.5);
setAngularZ(0.0);
sendCommand();
usleep(1000000);
}
void ControlNode::setLinearVector(double x, double y, double z)
{
setLinearX(x);
setLinearY(y);
setLinearZ(z);
}
