diff --git a/RELENTLESS/include/superstructure.hpp b/RELENTLESS/include/superstructure.hpp index ac39379..6d777c0 100644 --- a/RELENTLESS/include/superstructure.hpp +++ b/RELENTLESS/include/superstructure.hpp @@ -19,7 +19,10 @@ namespace superstruct { // Movement Methods - void driveChassis(double dist, bool useHeadingCorrection); + void driveChassisAsync(double dist, bool useHeadingCorrection); + void driveChassisSync(double dist, bool useHeadingCorrection); + void driveChasissWithMD(double dist, bool useHeadingCorrection, double waitUntilDist); + void turnChassis(double theta); void leftSwing(double theta); void rightSwing(double theta); diff --git a/RELENTLESS/src/superstructure.cpp b/RELENTLESS/src/superstructure.cpp index 336994b..6c2a6ee 100644 --- a/RELENTLESS/src/superstructure.cpp +++ b/RELENTLESS/src/superstructure.cpp @@ -53,11 +53,23 @@ namespace superstruct { // motion and stuff - void driveChassis(double dist, bool useHeadingCorrection) { + void driveChassisAsync(double dist, bool useHeadingCorrection) { //chassis.set_mode(ary::DRIVE); chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); } + void driveChassisSync(double dist, bool useHeadingCorrection) { + //chassis.set_mode(ary::DRIVE); + chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); + chassis.wait_drive(); + } + + void driveChasissWithMD(double dist, bool useHeadingCorrection, double waitUntilDist) { + //chassis.set_mode(ary::DRIVE); + chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); + chassis.wait_until(waitUntilDist); + } + void turnChassis(double theta) { //chassis.set_mode(ary::TURN); chassis.set_turn(theta, TURN_SPEED * turnScale);