Quasics2018Code/Hedy/src/Commands/Primitives/MoveForDistance.cpp
// Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
void MoveForDistance::Execute() {
	static constexpr double deltaDown = 0.95;
	static constexpr double deltaUp = 1.04;
	static constexpr double maxOffset = 1.2;
	static constexpr double minOffset = 0.8;
	const double leftDistance = fabs(Robot::driveBase->LeftEncoderDistance());
	const double rightDistance = fabs(Robot::driveBase->RightEncoderDistance());
	LOG("Left dist: " << std::setw(5) << leftDistance
		  << " Right dist: " << std::setw(5) << rightDistance
		  << " / Left power: " << std::setw(5) << leftPower
		  << " Right power: " << std::setw(5) << rightPower);
	if(leftDistance  > rightDistance) {
		double v = leftPower * deltaDown;
		if (fabs(v) >= fabs(power * minOffset)) {
			leftPower = v;
		} else {
			if (fabs(rightPower * deltaUp) <= fabs(power * maxOffset)) {
				rightPower = rightPower * deltaUp;
			} else {
				rightPower = power * maxOffset;
			}
		}		
		Robot::driveBase->SetPowerToMotors(-leftPower, rightPower);
	}
	else if(leftDistance < rightDistance){
		double v = rightPower * deltaDown;
		if (fabs(v) >= fabs(power * .8)) {
			rightPower = v;
		} else {
			if (fabs(leftPower * deltaUp) <= fabs(power * 1.2)) {
				leftPower = leftPower * deltaUp;
			} else {
				leftPower = power * 1.2;
			}
		}
		Robot::driveBase->SetPowerToMotors(-(leftPower), (rightPower));
	}
	else{
		Robot::driveBase->SetPowerToMotors(-(leftPower), rightPower);
	}
}
void MoveForDistance :: Execute ( ) { static constexpr double deltaDown = 0.95 ; static constexpr double deltaUp = 1.04 ; static constexpr double maxOffset = 1.2 ; static constexpr double minOffset = 0.8 ; const double leftDistance = fabs ( Robot :: driveBase -> LeftEncoderDistance ( ) ) ; const double rightDistance = fabs ( Robot :: driveBase -> RightEncoderDistance ( ) ) ; LOG ( " " << std :: setw ( 5 ) << leftDistance << " " << std :: setw ( 5 ) << rightDistance << " " << std :: setw ( 5 ) << leftPower << " " << std :: setw ( 5 ) << rightPower ) ; if ( leftDistance > rightDistance ) { double v = leftPower * deltaDown ; if ( fabs ( v ) >= fabs ( power * minOffset ) ) { leftPower = v ; } else { if ( fabs ( rightPower * deltaUp ) <= fabs ( power * maxOffset ) ) { rightPower = rightPower * deltaUp ; } else { rightPower = power * maxOffset ; } } Robot :: driveBase -> SetPowerToMotors ( - leftPower , rightPower ) ; } else if ( leftDistance < rightDistance ) { double v = rightPower * deltaDown ; if ( fabs ( v ) >= fabs ( power * .8 ) ) { rightPower = v ; } else { if ( fabs ( leftPower * deltaUp ) <= fabs ( power * 1.2 ) ) { leftPower = leftPower * deltaUp ; } else { leftPower = power * 1.2 ; } } Robot :: driveBase -> SetPowerToMotors ( - ( leftPower ) , ( rightPower ) ) ; } else { Robot :: driveBase -> SetPowerToMotors ( - ( leftPower ) , rightPower ) ; } }
Called repeatedly when this Command is scheduled to run
Called repeatedly when this Command is scheduled to run
// rightPower *= 1.1;