def update_outputs(self, l_enc: int, r_enc: int, angle: float): self.l_output = -self.left_follower.calculate(l_enc) self.r_output = self.right_follower.calculate(r_enc) if self.reverse: self.l_output = self.left_follower.calculate(-l_enc) self.r_output = -self.right_follower.calculate(-r_enc) if self.reverse: angle *= -1 desired_heading = pathfinder.r2d(self.left_follower.getHeading()) angle_diff = pathfinder.boundHalfDegrees(desired_heading - angle) angle_diff = angle_diff % 360 if abs(angle_diff) > 180: if angle_diff > 0: angle_diff = angle_diff - 360 else: angle_diff = angle_diff + 360 turn = 0.8 * (1.0 / 80.0) * angle_diff if not self.reverse: self.l_output -= turn self.r_output -= turn else: self.l_output += turn self.r_output += turn
def autonomousPeriodic(self): l = self.leftFollower.calculate( self.l_motor_master.getSelectedSensorPosition(0)) r = self.rightFollower.calculate( self.r_motor_master.getSelectedSensorPosition(0)) gyro_heading = (-self.navx.getAngle() ) # Assuming the gyro is giving a value in degrees desired_heading = pf.r2d( self.leftFollower.getHeading()) # Should also be in degrees # This is a poor man's P controller angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading) turn = 5 * (-1.0 / 80.0) * angleDifference if self.timer.hasPeriodPassed(0.5): print("l: " + str(l) + ", r: " + str(r) + ", turn: " + str(turn)) #print("current segment: " + str(self.leftFollower.getSegment().position)) l = l + turn r = r - turn if self.leftFollower.isFinished() or self.rightFollower.isFinished(): self.robot_drive.tankDrive(0, 0) else: # -1 is forward, so invert both values self.robot_drive.tankDrive(-l, -r)
def execute(self): if self.enabled: angle_speed = self.config[ "navx_p"] / 180 * pathfinder.boundHalfDegrees( pathfinder.r2d(self.left.getHeading()) - self.drive.getNavx()) self.drive.setSpeeds( self.left.calculate(self.drive.getEncLeft()) + (angle_speed if angle_speed < 0 else 0), self.right.calculate(self.drive.getEncRight()) - (angle_speed if angle_speed > 0 else 0))
def autonomousPeriodic(self): l = self.leftFollower.calculate(self.l_encoder.get()) r = self.rightFollower.calculate(self.r_encoder.get()) gyro_heading = (-self.gyro.getAngle() ) # Assuming the gyro is giving a value in degrees desired_heading = pf.r2d( self.leftFollower.getHeading()) # Should also be in degrees # This is a poor man's P controller angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading) turn = 5 * (-1.0 / 80.0) * angleDifference l = l + turn r = r - turn # -1 is forward, so invert both values self.robot_drive.tankDrive(-l, -r)
def charge(self, initial_call): left_speed = self.leftFollower.calculate(self.drive.leftDistance()) right_speed = self.rightFollower.calculate(self.drive.rightDistance()) gyro_heading = (-self.drive.gyro.getAngle() ) # Assuming the gyro is giving a value in degrees desired_heading = pf.r2d( self.leftFollower.getHeading()) # Should also be in degrees # This is a poor man's P controller angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading) turn = 1.2 * (-1.0 / 80.0) * angleDifference left_speed += turn right_speed -= turn print(left_speed, right_speed) # -1 is forward, so invert both values self.drive.tank(left_speed, right_speed)
def run(self): l = self.leftFollower.calculate(sself.sensors.getLeftDistance()) r = self.rightFollower.calculate(self.sensors.getRightDistance()) # REPLACE THIS WITH NAVX gyro_heading = -self.sensors.getCurrentAngle( ) # Assuming the gyro is giving a value in degrees desired_heading = pf.r2d( self.leftFollower.getHeading()) # Should also be in degrees # This is a poor man's P controller angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading) turn = 5 * (-1.0 / 80.0) * angleDifference l = l + turn r = r - turn # -1 is forward, so invert both values self.drivetrain().moveRobot(-l, -r)
def autonomousPeriodic(self): l = self.leftFollower.calculate(self.l_encoder.get()) r = self.rightFollower.calculate(self.r_encoder.get()) gyro_heading = ( -self.gyro.getAngle() ) # Assuming the gyro is giving a value in degrees desired_heading = pf.r2d( self.leftFollower.getHeading() ) # Should also be in degrees # This is a poor man's P controller angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading) turn = .8 * (-1.0 / 80.0) * angleDifference l = l + turn r = r - turn # -1 is forward, so invert both values self.robot_drive.tankDrive(-l, -r)
def exec_path(self): if self.finished: return print('[path controller] [current] L: %s; R: %s' % (self.drivetrain.get_left_encoder_meters(), self.drivetrain.get_right_encoder_meters())) l_dist = self.drivetrain.get_left_encoder_meters() \ - self.position_offset[0] r_dist = self.drivetrain.get_right_encoder_meters() \ - self.position_offset[1] if self.reverse: l_dist *= -1 r_dist *= -1 try: l_o = self.left.calculate(l_dist) r_o = self.right.calculate(r_dist) except Exception: return print('[path controller] [calculated] L: %s; R: %s' % (l_o, r_o)) gyro_heading = self.angle_controller.get_angle() - self.angle_offset desired_heading = -pf.r2d(self.left.getHeading()) if self.reverse: desired_heading += 180 print('[path controller] [heading] curr: %s, desired: %s' % (gyro_heading, desired_heading)) if not self.initial_desired_heading: self.initial_desired_heading = desired_heading angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading - self.initial_desired_heading) # TURN_FACTOR = 0.025 TURN_FACTOR = 0.01 turn = TURN_FACTOR * angleDifference if self.reverse: turn *= -1 # print('[path controller] [angle diff] %s [desired] %s [gyro] ' + # '%s [init desire] %s' % (angleDifference, desired_heading, # gyro_heading)) print('[path controller] [calculated w turn] L: %s; R: %s' % (l_o + turn, r_o - turn)) l_speed = l_o + turn r_speed = r_o - turn if self.reverse: l_speed *= -1 r_speed *= -1 self.drivetrain.manual_drive(l_speed, r_speed) if self.left.isFinished() and self.right.isFinished(): self.stop() self.finished = True