More work on skills auton

MillburnVex · Feb 22, 2019 · ee5ed5a · ee5ed5a
1 parent 9524de1
commit ee5ed5a
Show file tree

Hide file tree

Showing 4 changed files with 128 additions and 62 deletions.
diff --git a/src/Component.cpp b/src/Component.cpp
@@ -68,6 +68,10 @@ class IndexerComponent : public BotComponent {
 
 class DriveComponent : public BotComponent {
 
+	int ticksSinceLastMovement = -1;
+
+	const int TICKS_NOT_MOVING_TILL_CANCEL = 2500;
+
     const double LINEAR_TOTAL_ERROR_THRESHOLD = 45;
 
     const double ROTATE_TOTAL_ERROR_THRESHOLD = 10;
@@ -111,10 +115,10 @@ class DriveComponent : public BotComponent {
                 new std::pair<MotorID, double>(DRIVE_LEFT_BACK, 0)
     };
     std::array<std::pair < MotorID, PID> *, 4> linearPids{
-        new std::pair<MotorID, PID>(DRIVE_RIGHT_FRONT, PID(0.3f, 0.02f, 0.17f, 400, -400)),
-                new std::pair<MotorID, PID>(DRIVE_RIGHT_BACK, PID(0.3f, 0.02f, 0.17f, 400, -400)),
-                new std::pair<MotorID, PID>(DRIVE_LEFT_FRONT, PID(0.3f, 0.02f, 0.17f, 400, -400)),
-                new std::pair<MotorID, PID>(DRIVE_LEFT_BACK, PID(0.3f, 0.02f, 0.17f, 400, -400))
+        new std::pair<MotorID, PID>(DRIVE_RIGHT_FRONT, PID(0.31f, 0.02f, 0.17f, 400, -400)),
+                new std::pair<MotorID, PID>(DRIVE_RIGHT_BACK, PID(0.31f, 0.02f, 0.17f, 400, -400)),
+                new std::pair<MotorID, PID>(DRIVE_LEFT_FRONT, PID(0.31f, 0.02f, 0.17f, 400, -400)),
+                new std::pair<MotorID, PID>(DRIVE_LEFT_BACK, PID(0.31f, 0.02f, 0.17f, 400, -400))
     };
 
 public:
@@ -267,6 +271,21 @@ class DriveComponent : public BotComponent {
             ResetPIDS();
             return;
         }
+		if (NotMoving()) {
+			ticksSinceLastMovement++;
+			if (Robot::ShouldCancelCommandIfNotMoving() && ticksSinceLastMovement >= TICKS_NOT_MOVING_TILL_CANCEL) {
+				Commands::Release(C_DRIVE_LINEAR_TO);
+				printf("linear done           -----------------------\n");
+				UpdateGoalVoltages(0, 0);
+				Drive(0, 0);
+				UpdateInitialPositions();
+				ResetPIDS();
+				return;
+			}
+		}
+		else {
+			ticksSinceLastMovement = -1;
+		}
        /* printf("right error %f\n", std::abs(goalPositionRelative -
                                             (GetRelativePosition(DRIVE_RIGHT_FRONT) +
                                              GetRelativePosition(DRIVE_RIGHT_BACK)) / 2));
@@ -308,7 +327,21 @@ class DriveComponent : public BotComponent {
             goalRotation = target;
             return;
         }
-
+		if (NotMoving()) {
+			ticksSinceLastMovement++;
+			if (Robot::ShouldCancelCommandIfNotMoving() && ticksSinceLastMovement >= TICKS_NOT_MOVING_TILL_CANCEL) {
+				Commands::Release(C_DRIVE_LINEAR_TO);
+				printf("linear done           -----------------------\n");
+				UpdateGoalVoltages(0, 0);
+				Drive(0, 0);
+				UpdateInitialPositions();
+				ResetPIDS();
+				return;
+			}
+		}
+		else {
+			ticksSinceLastMovement = -1;
+		}
         printf("current value 1: %d\n", Robot::GetSensor(SensorID::GYRO)->GetValue());
 		printf("current value 2: %d\n", Robot::GetSensor(SensorID::GYRO_2)->GetValue());
 		printf("current value total: %d\n", Robot::GetRotation());

diff --git a/src/Robot.cpp b/src/Robot.cpp
@@ -6,6 +6,8 @@ int updateMillis = 5;
 
 int rotationOffset = 0;
 
+bool cancelCommandIfNotMoving = false;
+
 pros::Controller master = pros::Controller(pros::E_CONTROLLER_MASTER);
 pros::Controller partner = pros::Controller(pros::E_CONTROLLER_PARTNER);
 
@@ -34,6 +36,14 @@ void Robot::SetTeam(Team t) {
     team = t;
 }
 
+void Robot::SetCancelCommandIfNotMoving(bool t) {
+	cancelCommandIfNotMoving = t;
+}
+
+bool Robot::ShouldCancelCommandIfNotMoving() {
+	return cancelCommandIfNotMoving;
+}
+
 void Robot::SetAutonStrategy(Strategy newStrat) {
     strat = newStrat;
 }

diff --git a/src/Robot.h b/src/Robot.h
@@ -47,6 +47,10 @@ namespace Robot {
 
     int GetRotation();
 
+	void SetCancelCommandIfNotMoving(bool t);
+
+	bool ShouldCancelCommandIfNotMoving();
+
     void ResetRotation(int offset = 0);
 
     bool IsInManualMode();