TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( DefaultSpmdVectorSpace_Parallel, emptyProcSimpleMultiVecAdjointApply,
Scalar )
{
// typedef typename Teuchos::ScalarTraits<Scalar>::magnitudeType ScalarMag; // unused
// typedef Teuchos::ScalarTraits<ScalarMag> SMT; // unused
const Ordinal localDim = g_localDim;
PRINT_VAR(localDim);
const Ordinal numCols1 = g_numCols1;
const Ordinal numCols2= g_numCols2;
PRINT_VAR(numCols1);
PRINT_VAR(numCols2);
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createZeroEleProcVS<Scalar>(localDim);
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, numCols1);
const Scalar val1 = 2.0;
PRINT_VAR(val1);
ECHO(assign<Scalar>(mv.ptr(), val1));
out << "mv = " << *mv;
ECHO(const RCP<MultiVectorBase<Scalar> > Y = createMembers<Scalar>(mv->domain(), numCols2));
ECHO(const RCP<MultiVectorBase<Scalar> > X = createMembers<Scalar>(mv->range(), numCols2));
const Scalar val2 = 3.0;
PRINT_VAR(val2);
ECHO(assign<Scalar>(X.ptr(), val2));
out << "X = " << *X;
ECHO(apply<Scalar>(*mv, Thyra::CONJTRANS, *X, Y.ptr()));
out << "Y = " << *Y;
RTOpPack::ConstSubMultiVectorView<Scalar> Y_smvv =
Thyra::getLocalSubMultiVectorView<Scalar>(Y);
for (int i = 0; i < numCols1; ++i) {
for (int j = 0; j < numCols2; ++j) {
out << "i = " << i << ", j = " << j << ": ";
TEST_EQUALITY(Y_smvv(i,j), as<Scalar>(vs->dim())*val1*val2);
}
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SPMD_apply_op, vec_args_1_0_sum_zero_p1, Scalar )
{
const RCP<const Teuchos::Comm<Ordinal> > comm =
Teuchos::DefaultComm<Ordinal>::getComm();
const int procRank = rank(*comm);
const Ordinal localDim = (procRank == 1 ? 0 : g_localDim);
PRINT_VAR(g_localDim);
PRINT_VAR(localDim);
const Ordinal localOffset = computeLocalOffset(*comm, localDim);
const Scalar val = 1.1;
PRINT_VAR(val);
if (g_dumpRTOps) {
RTOpPack::set_SPMD_apply_op_dump_out(rcpFromRef(out));
}
RTOpPack::SubVectorView<Scalar> x =
getLocalSubVectorView<Scalar>(localOffset, localDim, val);
RTOpPack::ROpSum<Scalar> sumOp;
RCP<RTOpPack::ReductTarget> sumTarget = sumOp.reduct_obj_create();
RTOpPack::SPMD_apply_op<Scalar>(&*comm, sumOp, 1, &x, 0, 0, &*sumTarget);
Scalar sum_x = sumOp(*sumTarget);
const Ordinal procFactor = (comm->getSize() == 1 ? 1 : (comm->getSize()-1));
PRINT_VAR(procFactor);
TEST_EQUALITY(sum_x, as<Scalar>(g_localDim * procFactor) * val);
RTOpPack::set_SPMD_apply_op_dump_out(Teuchos::null);
}
void assertMultiVectorSums(const Teuchos::Comm<Ordinal> &comm,
const ConstSubMultiVectorView<Scalar> &mv,
const Ordinal localDim, const Scalar val, const int numProcsFact,
Teuchos::FancyOStream &out, bool &success)
{
const Ordinal numCols = mv.numSubCols();
RTOpPack::ROpSum<Scalar> sumOp;
Array<RCP<RTOpPack::ReductTarget> > sumTargets_store(numCols);
Array<RTOpPack::ReductTarget*> sumTargets(numCols);
for (int j = 0; j < numCols; ++j) {
sumTargets_store[j] = sumOp.reduct_obj_create();
sumTargets[j] = sumTargets_store[j].getRawPtr();
}
RTOpPack::SPMD_apply_op<Scalar>(&comm, sumOp, numCols, 1, &mv, 0, 0,
sumTargets.getRawPtr());
PRINT_VAR(localDim);
PRINT_VAR(val);
PRINT_VAR(numProcsFact);
for (int j = 0; j < numCols; ++j) {
PRINT_VAR(j);
Scalar sum_mv_j = sumOp(*sumTargets[j]);
TEST_EQUALITY(sum_mv_j, as<Scalar>(localDim*numProcsFact)*val);
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SPMD_apply_op, multivec_args_0_1_assign_zero_p0, Scalar )
{
const RCP<const Teuchos::Comm<Ordinal> > comm =
Teuchos::DefaultComm<Ordinal>::getComm();
if (g_dumpRTOps) {
RTOpPack::set_SPMD_apply_op_dump_out(rcpFromRef(out));
}
const int procRank = rank(*comm);
const Ordinal localDim = (procRank == 0 ? 0 : g_localDim);
PRINT_VAR(localDim);
const Ordinal numCols = 3;
PRINT_VAR(numCols);
const Ordinal localOffset = computeLocalOffset(*comm, localDim);
PRINT_VAR(localOffset);
const Scalar val_init = -0.1;
PRINT_VAR(val_init);
RTOpPack::SubMultiVectorView<Scalar> mv =
getLocalSubMultiVectorView<Scalar>(localOffset, localDim, numCols, val_init);
const Scalar val = 1.2;
PRINT_VAR(val);
RTOpPack::TOpAssignScalar<Scalar> assignOp(val);
RTOpPack::SPMD_apply_op<Scalar>(&*comm, assignOp, numCols, 0, 0, 1, &mv, 0);
assertMultiVectorSums<Scalar>(*comm, mv, g_localDim, val, comm->getSize()-1,
out, success);
RTOpPack::set_SPMD_apply_op_dump_out(Teuchos::null);
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
getLocalSubMultiVectorView_procRankLocalDim, Scalar )
{
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createProcRankLocalDimVS<Scalar>();
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const int procRank = comm->getRank();
PRINT_VAR(procRank);
const int numProcs = comm->getSize();
PRINT_VAR(numProcs);
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, g_numCols);
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
assign<Scalar>(mv.ptr(), val);
const ScalarMag tol = 100.0*ScalarTraits<Scalar>::eps();
TEST_EQUALITY_CONST(mv->domain()->dim(), g_numCols);
for (int j = 0; j < g_numCols; ++j) {
TEST_FLOATING_EQUALITY(sum<Scalar>(*mv->col(0)), as<Scalar>(vs->dim())*val, tol);
}
out << "*** Test that we get the view correctly ...\n";
RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(mv);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
TEST_EQUALITY(lsmv(i,j), val);
}
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
getNonconstLocalSubVectorView_empty_p0, Scalar )
{
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createZeroEleProcVS<Scalar>(g_localDim);
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const int procRank = comm->getRank();
PRINT_VAR(procRank);
const int numProcs = comm->getSize();
PRINT_VAR(numProcs);
const RCP<VectorBase<Scalar> > v = createMember<Scalar>(vs);
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
assign<Scalar>(v.ptr(), val);
out << "*** Test that we get the view correctly including an empty view on p0 ...\n";
RTOpPack::SubVectorView<Scalar> lsv =
getNonconstLocalSubVectorView<Scalar>(v);
if (procRank == 0) {
TEST_EQUALITY_CONST(lsv.globalOffset(), 0);
TEST_EQUALITY_CONST(lsv.subDim(), 0);
TEST_EQUALITY_CONST(lsv.values(), null);
}
else {
TEST_EQUALITY(lsv.globalOffset(), as<Ordinal>((procRank-1)*g_localDim));
TEST_EQUALITY(lsv.subDim(), g_localDim);
}
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
TEST_EQUALITY(lsv[k], val);
}
}
int main() {
struct _type_t var1, var2, var3, *var4;
_i32 i = 10, j = 20;
ast.env.n_vars = 0;
new_var(&ast, &var1, "variable1", 1, &i);
new_var(&ast, &var2, "variable2", 1, &j);
new_var(&ast, &var3, "str", STR, "hello world");
PRINT_INTEGER(var1);
PRINT_INTEGER(var2);
PRINT_STR(var3);
if (get_var(&ast, &var4, "variable1")) {
PRINT_VAR((*var4));
} else {
printf("failed to get variable\n");
}
assign(&ast, "variable1", 15);
PRINT_VAR(var1);
assign(&ast, "variable1", 20);
PRINT_VAR(var1);
return 0;
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( DefaultSpmdVectorSpace_Parallel, emptyProcPrintMultiVec,
Scalar )
{
const Ordinal localDim = g_localDim;
PRINT_VAR(localDim);
const Ordinal numCols = 3;
PRINT_VAR(numCols);
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs = createZeroEleProcVS<Scalar>(localDim);
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, numCols);
ECHO(assign(mv.ptr(), as<Scalar>(1.5)));
out << "mv = " << *mv;
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
getLocalSubVectorView_procRankLocalDim, Scalar )
{
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createProcRankLocalDimVS<Scalar>();
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const int procRank = comm->getRank();
PRINT_VAR(procRank);
const int numProcs = comm->getSize();
PRINT_VAR(numProcs);
TEST_EQUALITY(vs->isLocallyReplicated(), numProcs==1);
out << "*** A) Get view directly through an SPMD Vector object ...\n";
{
const RCP<VectorBase<Scalar> > v = createMember<Scalar>(vs);
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
assign<Scalar>(v.ptr(), val);
const ScalarMag tol = 100.0*ScalarTraits<Scalar>::eps();
TEST_FLOATING_EQUALITY(sum<Scalar>(*v), as<Scalar>(vs->dim())*val, tol);
RTOpPack::ConstSubVectorView<Scalar> lsv =
getLocalSubVectorView<Scalar>(v);
TEST_EQUALITY(lsv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsv.subDim(), procRank+1);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
TEST_EQUALITY(lsv[k], val);
}
}
out << "*** B) Get view indirectly through a product vector with one block ...\n";
{
const RCP<const VectorSpaceBase<Scalar> > pvs =
productVectorSpace<Scalar>(tuple<RCP<const VectorSpaceBase<Scalar> > >(vs)());
const RCP<VectorBase<Scalar> > pv = createMember<Scalar>(pvs);
const Scalar val = as<Scalar>(1.7);
PRINT_VAR(val);
assign<Scalar>(pv.ptr(), val);
RTOpPack::ConstSubVectorView<Scalar> lsv =
getLocalSubVectorView<Scalar>(pv);
TEST_EQUALITY(lsv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsv.subDim(), procRank+1);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
TEST_EQUALITY(lsv[k], val);
}
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
getNonconstLocalSubMultiVectorView_empty_p0, Scalar )
{
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createZeroEleProcVS<Scalar>(g_localDim);
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const int procRank = comm->getRank();
PRINT_VAR(procRank);
const int numProcs = comm->getSize();
PRINT_VAR(numProcs);
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, g_numCols);
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
assign<Scalar>(mv.ptr(), val);
{
out << "*** Test that we get and change the nonconst view correctly ...\n";
RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(mv);
if (procRank == 0) {
TEST_EQUALITY_CONST(lsmv.globalOffset(), 0);
TEST_EQUALITY_CONST(lsmv.subDim(), 0);
TEST_EQUALITY_CONST(lsmv.values(), null);
}
else {
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank-1)*g_localDim));
TEST_EQUALITY(lsmv.subDim(), g_localDim);
}
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
lsmv(i,j) = lsmv.globalOffset() + i + 0.1 * j;
}
}
}
{
out << "*** Test that we get the same values when we grab const view ...\n";
RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(mv);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
TEST_EQUALITY(lsmv(i,j), as<Scalar>(lsmv.globalOffset() + i + 0.1 * j));
}
}
}
}
int main (void)
{
int i = 42 ;
float f = 0.42 ;
char * s = "Hello, world!" ;
PRINT_IN_COLOR (COLOR_YELLOW, "%s\n", s) ;
PRINT_IN_COLOR (COLOR_CYAN, "I'm blue!\n") ;
PRINT_IN_COLOR (COLOR_RED, "Nope! You are cyan...\n") ;
PRINT_IN_COLOR (COLOR_GREEN, "%d is my favorite number.\n", i) ;
PRINT_IN_COLOR (COLOR_MAGENTA, "But I like %09f too.\n", f) ;
PRINT_VAR (i, print_int) ; printf ("\n") ;
PRINT_VAR (f, print_float) ; printf ("\n") ;
PRINT_VAR (s, print_string) ; printf ("\n") ;
return 0 ;
}
int main(void)
{
UNDERLINE_VAR(a);
_a = 3;
PRINT_VAR(_a);
PRINT_RED("f**k %s\n", "you");
return 0;
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( DefaultSpmdVectorSpace_Parallel, emptyProcAssignSumMultiVec,
Scalar )
{
const Ordinal localDim = g_localDim;
PRINT_VAR(localDim);
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs = createZeroEleProcVS<Scalar>(localDim);
const Ordinal numCols = 3;
PRINT_VAR(numCols);
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, numCols);
const Scalar val = 1.5;
PRINT_VAR(val);
ECHO(assign(mv.ptr(), as<Scalar>(val)));
Teuchos::Array<Scalar> sums(numCols);
Thyra::sums<Scalar>(*mv, sums());
for (int j = 0; j < numCols; ++j) {
PRINT_VAR(j);
TEST_EQUALITY(sums[j],
as<Scalar>(localDim*(vs->getComm()->getSize()-1))*val);
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
zeroVS, Scalar )
{
out << "Create a locally replicated vector space ...\n";
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createZeroVS<Scalar>();
TEST_ASSERT(!vs->isLocallyReplicated());
TEST_EQUALITY_CONST(vs->dim(), 0);
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
out << "Test locally replicated Vector ...\n";
const RCP<VectorBase<Scalar> > v = createMember<Scalar>(vs);
{
ECHO(RTOpPack::SubVectorView<Scalar> lsv =
getNonconstLocalSubVectorView<Scalar>(v));
TEST_EQUALITY_CONST(lsv.globalOffset(), 0);
TEST_EQUALITY(lsv.subDim(), 0);
TEST_EQUALITY_CONST(lsv.stride(), 1);
}
{
ECHO(RTOpPack::ConstSubVectorView<Scalar> lsv =
getLocalSubVectorView<Scalar>(v));
TEST_EQUALITY_CONST(lsv.globalOffset(), 0);
TEST_EQUALITY(lsv.subDim(), 0);
TEST_EQUALITY_CONST(lsv.stride(), 1);
}
assign<Scalar>(v.ptr(), val);
TEST_EQUALITY(sum<Scalar>(*v), as<Scalar>(0.0));
out << "Test locally replicated MultiVector ...\n";
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, g_numCols);
{
ECHO(RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(mv));
TEST_EQUALITY(lsmv.globalOffset(), 0);
TEST_EQUALITY(lsmv.subDim(), 0);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
}
{
ECHO(RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(mv));
}
assign<Scalar>(mv.ptr(), val);
for (int j = 0; j < g_numCols; ++j) {
TEST_EQUALITY(sum<Scalar>(*mv->col(0)), as<Scalar>(0.0));
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SPMD_apply_op, multivec_args_1_0_sum, Scalar )
{
const RCP<const Teuchos::Comm<Ordinal> > comm =
Teuchos::DefaultComm<Ordinal>::getComm();
//const int procRank = rank(*comm);
const Ordinal localDim = g_localDim;
PRINT_VAR(localDim);
const Ordinal numCols = 3;
PRINT_VAR(numCols);
const Ordinal localOffset = computeLocalOffset(*comm, localDim);
PRINT_VAR(localOffset);
const Scalar val = 1.1;
PRINT_VAR(val);
RTOpPack::SubMultiVectorView<Scalar> mv =
getLocalSubMultiVectorView<Scalar>(localOffset, localDim, numCols, val);
assertMultiVectorSums<Scalar>(*comm, mv, g_localDim, val, comm->getSize(),
out, success);
}
void emptyProcVectorSpaceTester(const int rootRank, FancyOStream &out, bool &success)
{
const Ordinal localDim = g_localDim;
PRINT_VAR(localDim);
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createZeroEleProcVS<Scalar>(localDim, rootRank);
typedef Teuchos::ScalarTraits<Scalar> ST;
typedef typename ST::magnitudeType ScalarMag;
ScalarMag tol = as<ScalarMag>(100.0) * ScalarTraits<ScalarMag>::eps();
Thyra::VectorSpaceTester<Scalar> vectorSpaceTester;
vectorSpaceTester.warning_tol(ScalarMag(0.1)*tol);
vectorSpaceTester.error_tol(tol);
vectorSpaceTester.show_all_tests(g_show_all_tests);
vectorSpaceTester.dump_all(g_dump_objects);
Thyra::VectorStdOpsTester<Scalar> vectorStdOpsTester;
vectorStdOpsTester.warning_tol(ScalarMag(0.1)*tol);
vectorStdOpsTester.error_tol(tol);
Thyra::MultiVectorStdOpsTester<Scalar> multiVectorStdOpsTester;
multiVectorStdOpsTester.warning_tol(ScalarMag(0.1)*tol);
multiVectorStdOpsTester.error_tol(tol);
if (g_dumpRTOps) {
RTOpPack::set_SPMD_apply_op_dump_out(rcpFromRef(out));
}
out << "\nTesting the VectorSpaceBase interface of vs ...\n";
TEST_ASSERT(vectorSpaceTester.check(*vs, &out));
out << "\nTesting standard vector ops for vs ...\n";
TEST_ASSERT(vectorStdOpsTester.checkStdOps(*vs, &out));
out << "\nTesting standard multivector ops for vs ...\n";
TEST_ASSERT(multiVectorStdOpsTester.checkStdOps(*vs, &out));
RTOpPack::set_SPMD_apply_op_dump_out(Teuchos::null);
}
void Navigator::handleSubsumptionEvent( EventNotification* pEvent, SubsumptionParams* pSubsumptionParams )
{
float headingToWaypoint;
float headingError;
int distanceToWaypoint;
if ( _bEnabled ) {
// now, if this event has not already been subsumed, we need to plot a course
// from our current position to our current waypoint, if any.
if ( ! pSubsumptionParams->ControlFreak() ) {
// adjust the motors' speeds as necessary to correct our heading
headingToWaypoint = _pPosition->_theta; // current heading in radians, in case we don't have a waypoint
if ( _pCurrentWaypoint ) {
// compute distance to target
float dx = _pCurrentWaypoint->_x - _pPosition->_xInches;
float dy = _pCurrentWaypoint->_y - _pPosition->_yInches;
distanceToWaypoint = sqrt( dx * dx + dy * dy ); // thank you, Mr. Pythagoras
// If we're close enough to this waypoint, move to the next
if ( distanceToWaypoint < _pCurrentWaypoint->_radius ) {
_pCurrentWaypoint = _pWaypointManager->GetWaypoint( ++_waypointNumber );
_bCorrecting = false;
PROGRESS_MSG( "\nNext Waypoint\n" );
if ( !_pCurrentWaypoint ) {
// no further waypoints, so shut down and take control
PROGRESS_MSG( "\nWe have arrived!\n" );
pSubsumptionParams->SetThrottles( 0, 0, this);
_waypointNumber = 0;
}
}
else {
// compute heading to current waypoint
// note that atan2() calls for dy/dx, but that yields angles referenced to the
// x-axis, or 0 = East. For navigation, we want 0 = North, so we swap the
// arguments to get the correct alignment.
headingToWaypoint = atan2( dx, dy );
IF_MASK( MM_CALC ) {
PRINT_VAR( dx );
PRINT_VAR( dy );
PRINT_VAR( headingToWaypoint );
}
headingError = _pPosition->_theta - headingToWaypoint;
IF_MASK( MM_CALC ) {
PRINT_VAR( headingError );
}
// normalize the error value
float piOffset = headingError < 0.0 ? -PI : PI;
headingError = fmod( headingError + piOffset, 2.0 * PI ) - PI;
// headingError = atan( tan( headingError ) );
IF_MASK( MM_CALC ) {
Serial.print( F("Adjusted ") );
PRINT_VAR( headingError );
}
// if heading is outside our tolerance band, perform correction
if ( fabs( headingError ) > _headingTolerance ) {
// _bCorrecting means we already have a current snapshot
if ( ! _bCorrecting ) {
_bCorrecting = true;
// snapshot current throttle positions as a baseline
_leftThrottleSnapshot = pSubsumptionParams->GetLeftThrottle();
_rightThrottleSnapshot = pSubsumptionParams->GetRightThrottle();
IF_MASK( MM_CALC ) {
PRINT_VAR( _leftThrottleSnapshot );
PRINT_VAR( _rightThrottleSnapshot );
}
}
// negative error means too far left, so slow the right motor
if ( headingError < 0 ) {
// map error (0..3) to throttle ( rightsnapshot .. -leftsnapshot )
int rightThrottle = fmap( -headingError, 0.0, 3.14, _rightThrottleSnapshot, -_leftThrottleSnapshot );
pSubsumptionParams->SetThrottles( _leftThrottleSnapshot, rightThrottle , this);
IF_MASK( MM_CALC ) {
PRINT_VAR( rightThrottle );
}
}
else {
int leftThrottle = fmap( headingError, 0.0, 3.14, _leftThrottleSnapshot, -_rightThrottleSnapshot );
pSubsumptionParams->SetThrottles( leftThrottle, _rightThrottleSnapshot, this);
IF_MASK( MM_CALC ) {
PRINT_VAR( leftThrottle );
}
}
}
else {
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
getNonconstLocalSubMultiVectorView_procRankLocalDim, Scalar )
{
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createProcRankLocalDimVS<Scalar>();
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const int procRank = comm->getRank();
PRINT_VAR(procRank);
const int numProcs = comm->getSize();
PRINT_VAR(numProcs);
out << "*** A) Test getting nonconst MV view directly from SPMD MultiVector ...\n";
{
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, g_numCols);
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
assign<Scalar>(mv.ptr(), val);
{
out << "*** A.1) Get and change the nonconst view ...\n";
RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(mv);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
lsmv(i,j) = lsmv.globalOffset() + i + 0.1 * j;
}
}
}
{
out << "*** A.2) Get the same values when we grab const view ...\n";
RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(mv);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
TEST_EQUALITY(lsmv(i,j), as<Scalar>(lsmv.globalOffset() + i + 0.1 * j));
}
}
}
}
out << "*** B) Test getting nonconst MV view indirectly from one-block"
<< " Product MultiVector ...\n";
{
const RCP<const VectorSpaceBase<Scalar> > pvs =
productVectorSpace<Scalar>(tuple<RCP<const VectorSpaceBase<Scalar> > >(vs)());
const RCP<MultiVectorBase<Scalar> > pmv = createMembers<Scalar>(pvs, g_numCols);
const Scalar val = as<Scalar>(1.8);
PRINT_VAR(val);
assign<Scalar>(pmv.ptr(), val);
{
out << "*** B.1) Get and change the nonconst view ...\n";
RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(pmv);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
lsmv(i,j) = lsmv.globalOffset() + i + 0.1 * j;
}
}
}
{
out << "*** B.2) Get the same values when we grab const view ...\n";
RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(pmv);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
TEST_EQUALITY(lsmv(i,j), as<Scalar>(lsmv.globalOffset() + i + 0.1 * j));
}
}
}
}
out << "*** C) Test getting nonconst MV view directly from SPMD Vector ...\n";
if (1) {
const RCP<VectorBase<Scalar> > v = createMember<Scalar>(vs);
const Scalar val = as<Scalar>(2.1);
PRINT_VAR(val);
assign<Scalar>(v.ptr(), val);
{
out << "*** C.1) Get and change the nonconst MV view ...\n";
RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(v);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY_CONST(lsmv.numSubCols(), 1);
for (int i = 0; i < lsmv.subDim(); ++i) {
lsmv(i,0) = lsmv.globalOffset() + i;
}
}
{
out << "*** C.2) Get the same values when we grab const MV view ...\n";
RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(v);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY_CONST(lsmv.numSubCols(), 1);
for (int i = 0; i < lsmv.subDim(); ++i) {
TEST_EQUALITY(lsmv(i,0), as<Scalar>(lsmv.globalOffset() + i));
}
}
}
out << "*** D) Test getting nonconst MV view indirectly from one-block"
<< " Product MultiVector ...\n";
{
const RCP<const VectorSpaceBase<Scalar> > pvs =
productVectorSpace<Scalar>(tuple<RCP<const VectorSpaceBase<Scalar> > >(vs)());
const RCP<VectorBase<Scalar> > pv = createMember<Scalar>(pvs);
const Scalar val = as<Scalar>(1.8);
PRINT_VAR(val);
assign<Scalar>(pv.ptr(), val);
{
out << "*** D.1) Get and change the nonconst view ...\n";
RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(pv);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), 1);
for (int i = 0; i < lsmv.subDim(); ++i) {
lsmv(i,0) = lsmv.globalOffset() + i;
}
}
{
out << "*** D.2) Get the same values when we grab const view ...\n";
RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(pv);
TEST_EQUALITY(lsmv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsmv.subDim(), procRank+1);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), 1);
for (int i = 0; i < lsmv.subDim(); ++i) {
TEST_EQUALITY(lsmv(i,0), as<Scalar>(lsmv.globalOffset() + i));
}
}
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
locallyReplicated, Scalar )
{
out << "Create a locally replicated vector space ...\n";
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createLocallyReplicatedVS<Scalar>(g_localDim);
TEST_EQUALITY(vs->dim(), g_localDim);
TEST_ASSERT(vs->isLocallyReplicated());
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
out << "Test locally replicated Vector ...\n";
const RCP<VectorBase<Scalar> > v = createMember<Scalar>(vs);
{
ECHO(RTOpPack::SubVectorView<Scalar> lsv =
getNonconstLocalSubVectorView<Scalar>(v));
TEST_EQUALITY_CONST(lsv.globalOffset(), 0);
TEST_EQUALITY(lsv.subDim(), g_localDim);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
lsv[k] = k + 1;
}
}
{
ECHO(RTOpPack::ConstSubVectorView<Scalar> lsv =
getLocalSubVectorView<Scalar>(v));
TEST_EQUALITY_CONST(lsv.globalOffset(), 0);
TEST_EQUALITY(lsv.subDim(), g_localDim);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
TEST_EQUALITY(lsv[k], as<Scalar>(lsv.globalOffset() + k + 1));
}
}
out << "Test locally replicated MultiVector ...\n";
const RCP<MultiVectorBase<Scalar> > mv = createMembers<Scalar>(vs, g_numCols);
{
ECHO(RTOpPack::SubMultiVectorView<Scalar> lsmv =
getNonconstLocalSubMultiVectorView<Scalar>(mv));
TEST_EQUALITY(lsmv.globalOffset(), 0);
TEST_EQUALITY(lsmv.subDim(), g_localDim);
TEST_EQUALITY(lsmv.leadingDim(), lsmv.subDim());
TEST_EQUALITY_CONST(lsmv.colOffset(), 0);
TEST_EQUALITY(lsmv.numSubCols(), g_numCols);
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
lsmv(i,j) = i + 0.1 * j;
}
}
}
{
ECHO(RTOpPack::ConstSubMultiVectorView<Scalar> lsmv =
getLocalSubMultiVectorView<Scalar>(mv));
for (int i = 0; i < lsmv.subDim(); ++i) {
for (int j = 0; j < lsmv.numSubCols(); ++j) {
TEST_EQUALITY(lsmv(i,j), as<Scalar>(i + 0.1 * j));
}
}
}
}
// if we were not already cruising, but have just assumed control (i.e., no other
// behavior has asserted itself this interval), note the Position readings so
// we can maintain our heading by keeping the same differential.
void CruiseControl::handleSubsumptionEvent( EventNotification* pEvent, SubsumptionParams* pSubsumptionParams )
{
// nothing to do if we're not enabled
if ( _bEnabled ) {
SubsumptionParams* pSubsumptionParams = (SubsumptionParams*) pEvent->pData;
if ( ! pSubsumptionParams->ControlFreak() ) {
_bCruising = false;
}
else { // nobody else cares, so it's our turn
float targetSpeedInchesPerInterval = ( _targetSpeedIPS * pSubsumptionParams->GetInterval() ) / 1000;
if ( _bCruising ) { // this means we were already cruising
// check our position and calculate error values
// delta is how far we have moved in this interval
float deltaLeft = _pPosition->_leftInches - _prevPositionLeft;
float deltaRight = _pPosition->_rightInches - _prevPositionRight;
// error is the difference between how far we expected to move and how far we actually moved.
float errorInchesLeft = targetSpeedInchesPerInterval - deltaLeft;
float errorInchesRight = targetSpeedInchesPerInterval - deltaRight;
// Derivative uses the change in error between the last two intervals
float deltaErrorLeft = _prevErrorLeft - errorInchesLeft;
float deltaErrorRight = _prevErrorRight - errorInchesRight;
// Integral term uses error in absolute position
float cumulativeErrorLeft = _idealPositionLeft - _pPosition->_leftInches;
float cumulativeErrorRight = _idealPositionRight - _pPosition->_rightInches;
// calculate new throttle positions using PID
_throttleLeft += ( ( _kP * errorInchesLeft ) + ( _kI * cumulativeErrorLeft ) + ( _kD * deltaErrorLeft ) );
_throttleRight += ( ( _kP * errorInchesRight ) + ( _kI * cumulativeErrorRight ) + ( _kD * deltaErrorRight ) );
// Show our work
if ( _messageMask & MM_PROGRESS ) {
Serial.println( F( "\nCruise Control PID calc:" ) );
PRINT_VAR( _idealPositionLeft );
PRINT_VAR( _pPosition->_leftInches );
PRINT_VAR( _prevPositionLeft );
PRINT_VAR( deltaLeft );
PRINT_VAR( targetSpeedInchesPerInterval );
PRINT_VAR( errorInchesLeft );
PRINT_VAR( cumulativeErrorLeft );
PRINT_VAR( _kI * cumulativeErrorLeft );
}
IF_CSV( MM_CSVBASIC ) {
CSV_OUT( targetSpeedInchesPerInterval );
CSV_OUT( _idealPositionLeft );
CSV_OUT( _prevPositionLeft );
CSV_OUT( deltaLeft );
CSV_OUT( errorInchesLeft );
CSV_OUT( cumulativeErrorLeft );
CSV_OUT( _kI * cumulativeErrorLeft );
CSV_OUT( _idealPositionRight );
CSV_OUT( _prevPositionRight );
CSV_OUT( deltaRight );
CSV_OUT( errorInchesRight );
CSV_OUT( cumulativeErrorRight );
CSV_OUT( _kI * cumulativeErrorRight );
}
// Upate some numbers for the next time
_prevErrorLeft = errorInchesLeft;
_prevErrorRight = errorInchesRight;
_prevPositionLeft = _pPosition->_leftInches;
_prevPositionRight = _pPosition->_rightInches;
}
else { // we just took control, so let's cruise!
_bCruising = true;
// set up for 0 errors next pass
_prevPositionLeft = _idealPositionLeft = _pPosition->_leftInches;
_prevPositionRight = _idealPositionRight = _pPosition->_rightInches;
}
// set the next ideal target positions
_idealPositionLeft += targetSpeedInchesPerInterval;
_idealPositionRight += targetSpeedInchesPerInterval;
// set the throttle positions.
pSubsumptionParams->SetThrottles( _throttleLeft, _throttleRight, this );
}
}
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( SpmdLocalDataAccess,
getNonconstLocalSubVectorView_procRankLocalDim, Scalar )
{
typedef typename ScalarTraits<Scalar>::magnitudeType ScalarMag;
const RCP<const DefaultSpmdVectorSpace<Scalar> > vs =
createProcRankLocalDimVS<Scalar>();
const RCP<const Teuchos::Comm<Ordinal> > comm = vs->getComm();
const int procRank = comm->getRank();
PRINT_VAR(procRank);
const int numProcs = comm->getSize();
PRINT_VAR(numProcs);
out << "*** A) Test that we get and change the nonconst view"
<< " directly through an SPMD Vector ...\n";
{
const RCP<VectorBase<Scalar> > v = createMember<Scalar>(vs);
const Scalar val = as<Scalar>(1.5);
PRINT_VAR(val);
assign<Scalar>(v.ptr(), val);
const ScalarMag tol = 100.0*ScalarTraits<Scalar>::eps();
TEST_FLOATING_EQUALITY(sum<Scalar>(*v), as<Scalar>(vs->dim())*val, tol);
{
out << "*** A.1) Get the non-const view and set the local elements ...\n";
RTOpPack::SubVectorView<Scalar> lsv =
getNonconstLocalSubVectorView<Scalar>(v);
TEST_EQUALITY(lsv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsv.subDim(), procRank+1);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
lsv[k] = lsv.globalOffset() + k + 1;
}
const Ordinal n = vs->dim();
TEST_FLOATING_EQUALITY(sum<Scalar>(*v), as<Scalar>((n*(n+1))/2.0), tol);
}
{
out << "*** A.2) Get the const view and check the local elemetns ...\n";
RTOpPack::ConstSubVectorView<Scalar> lsv =
getLocalSubVectorView<Scalar>(v);
TEST_EQUALITY(lsv.subDim(), procRank+1);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
TEST_EQUALITY(lsv[k], as<Scalar>(lsv.globalOffset() + k + 1));
}
}
}
out << "*** B) Test that we get and change the nonconst view"
<< " indirectly through a product vector with one block ...\n";
{
const RCP<const VectorSpaceBase<Scalar> > pvs =
productVectorSpace<Scalar>(tuple<RCP<const VectorSpaceBase<Scalar> > >(vs)());
const RCP<VectorBase<Scalar> > pv = createMember<Scalar>(pvs);
const Scalar val = as<Scalar>(1.7);
PRINT_VAR(val);
assign<Scalar>(pv.ptr(), val);
const ScalarMag tol = 100.0*ScalarTraits<Scalar>::eps();
{
out << "*** B.1) Get the non-const view and set the local elements ...\n";
RTOpPack::SubVectorView<Scalar> lsv =
getNonconstLocalSubVectorView<Scalar>(pv);
TEST_EQUALITY(lsv.globalOffset(), as<Ordinal>((procRank*(procRank+1))/2));
TEST_EQUALITY(lsv.subDim(), procRank+1);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
lsv[k] = lsv.globalOffset() + k + 1;
}
const Ordinal n = vs->dim();
TEST_FLOATING_EQUALITY(sum<Scalar>(*pv), as<Scalar>((n*(n+1))/2.0), tol);
}
{
out << "*** B.2) Get the const view and check the local elemetns ...\n";
RTOpPack::ConstSubVectorView<Scalar> lsv =
getLocalSubVectorView<Scalar>(pv);
TEST_EQUALITY(lsv.subDim(), procRank+1);
TEST_EQUALITY_CONST(lsv.stride(), 1);
for (int k = 0; k < lsv.subDim(); ++k) {
TEST_EQUALITY(lsv[k], as<Scalar>(lsv.globalOffset() + k + 1));
}
}
}
}
// if we were not already cruising, but have just assumed control (i.e., no other
// behavior has asserted itself this interval), note the Position readings so
// we can maintain our heading by keeping the same differential.
void CruiseControl::handleControlEvent( EventNotification* pEvent, MotorParams* pMotorParams )
{
// nothing to do if we're not enabled
if ( _bEnabled ) {
MotorParams* pMotorParams = (MotorParams*) pEvent->pData;
if ( NULL != pMotorParams->_pTakenBy ) {
_bCruising = false;
}
else { // nobody else cares, so it's our turn
pMotorParams->_pTakenBy = this;
if ( _bCruising ) { // this means we were already cruising
// check our position and calculate error values
// delta is how far we have moved in this interval
int deltaLeft = _pPosition->_currentEncoderPositionLeft - _prevPositionLeft;
int deltaRight = _pPosition->_currentEncoderPositionRight - _prevPositionRight;
// error is the difference between how far we expected to move and how far we actually moved.
int errorLeft = _targetSpeedTicks - deltaLeft;
int errorRight = _targetSpeedTicks - deltaRight;
// Derivative uses the change in error between the last two intervals
int deltaErrorLeft = _prevErrorLeft - errorLeft;
int deltaErrorRight = _prevErrorRight - errorRight;
// Integral term uses error in absolute position
int cumulativeErrorLeft = _idealPositionLeft - _pPosition->_currentEncoderPositionLeft;
int cumulativeErrorRight = _idealPositionRight - _pPosition->_currentEncoderPositionRight;
// calculate new throttle positions using PID
_throttleLeft += ( ( _kP * errorLeft ) + ( _kI * cumulativeErrorLeft ) + ( _kD * deltaErrorLeft ) );
_throttleRight += ( ( _kP * errorRight ) + ( _kI * cumulativeErrorRight ) + ( _kD * deltaErrorRight ) );
// Show our work
if ( _verbosityLevel >= 2 ) {
Serial.println( F( "\nCruise Control PID calc:" ) );
PRINT_VAR( _idealPositionLeft );
PRINT_VAR( _pPosition->_currentEncoderPositionLeft );
PRINT_VAR( _prevPositionLeft );
PRINT_VAR( deltaLeft );
PRINT_VAR( _targetSpeedTicks );
PRINT_VAR( errorLeft );
PRINT_VAR( cumulativeErrorLeft );
PRINT_VAR( _kI * cumulativeErrorLeft );
}
// Upsate some numbers for the next time
_prevErrorLeft = errorLeft;
_prevErrorRight = errorRight;
_prevPositionLeft = _pPosition->_currentEncoderPositionLeft;
_prevPositionRight = _pPosition->_currentEncoderPositionRight;
}
else { // we just took control, so let's cruise!
_bCruising = true;
// set up for 0 errors next pass
_prevPositionLeft = _idealPositionLeft = _pPosition->_currentEncoderPositionLeft;
_prevPositionRight = _idealPositionRight = _pPosition->_currentEncoderPositionRight;
}
// set the next ideal target positions
_idealPositionLeft += _targetSpeedTicks;
_idealPositionRight += _targetSpeedTicks;
// set the throttle positions.
pMotorParams->_throttleLeft = _throttleLeft;
pMotorParams->_throttleRight = _throttleRight;
//Serial.print( F( "Throttles set to: " ) );
//Serial.print( pMotorParams->_throttleLeft );
//Serial.print( '/' );
//Serial.println( pMotorParams->_throttleRight );
}
}
}
