mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
[wpilib] DifferentialDrive: Remove right side inversion (#3340)
Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
This commit is contained in:
Tyler Veness
@@ -0,0 +1,240 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.drive;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class DifferentialDriveTest {
|
||||
@Test
|
||||
void testArcadeDrive() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var drive = new DifferentialDrive(left, right);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.arcadeDrive(1.0, 0.0, false);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward left turn
|
||||
drive.arcadeDrive(0.5, -0.5, false);
|
||||
assertEquals(0.0, left.get(), 1e-9);
|
||||
assertEquals(0.5, right.get(), 1e-9);
|
||||
|
||||
// Forward right turn
|
||||
drive.arcadeDrive(0.5, 0.5, false);
|
||||
assertEquals(0.5, left.get(), 1e-9);
|
||||
assertEquals(0.0, right.get(), 1e-9);
|
||||
|
||||
// Backward
|
||||
drive.arcadeDrive(-1.0, 0.0, false);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward left turn
|
||||
drive.arcadeDrive(-0.5, -0.5, false);
|
||||
assertEquals(-0.5, left.get(), 1e-9);
|
||||
assertEquals(0.0, right.get(), 1e-9);
|
||||
|
||||
// Backward right turn
|
||||
drive.arcadeDrive(-0.5, 0.5, false);
|
||||
assertEquals(0.0, left.get(), 1e-9);
|
||||
assertEquals(-0.5, right.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testArcadeDriveSquared() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var drive = new DifferentialDrive(left, right);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.arcadeDrive(1.0, 0.0, true);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward left turn
|
||||
drive.arcadeDrive(0.5, -0.5, true);
|
||||
assertEquals(0.0, left.get(), 1e-9);
|
||||
assertEquals(0.25, right.get(), 1e-9);
|
||||
|
||||
// Forward right turn
|
||||
drive.arcadeDrive(0.5, 0.5, true);
|
||||
assertEquals(0.25, left.get(), 1e-9);
|
||||
assertEquals(0.0, right.get(), 1e-9);
|
||||
|
||||
// Backward
|
||||
drive.arcadeDrive(-1.0, 0.0, true);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward left turn
|
||||
drive.arcadeDrive(-0.5, -0.5, true);
|
||||
assertEquals(-0.25, left.get(), 1e-9);
|
||||
assertEquals(0.0, right.get(), 1e-9);
|
||||
|
||||
// Backward right turn
|
||||
drive.arcadeDrive(-0.5, 0.5, true);
|
||||
assertEquals(0.0, left.get(), 1e-9);
|
||||
assertEquals(-0.25, right.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testCurvatureDrive() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var drive = new DifferentialDrive(left, right);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.curvatureDrive(1.0, 0.0, false);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward left turn
|
||||
drive.curvatureDrive(0.5, -0.5, false);
|
||||
assertEquals(0.25, left.get(), 1e-9);
|
||||
assertEquals(0.75, right.get(), 1e-9);
|
||||
|
||||
// Forward right turn
|
||||
drive.curvatureDrive(0.5, 0.5, false);
|
||||
assertEquals(0.75, left.get(), 1e-9);
|
||||
assertEquals(0.25, right.get(), 1e-9);
|
||||
|
||||
// Backward
|
||||
drive.curvatureDrive(-1.0, 0.0, false);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward left turn
|
||||
drive.curvatureDrive(-0.5, -0.5, false);
|
||||
assertEquals(-0.75, left.get(), 1e-9);
|
||||
assertEquals(-0.25, right.get(), 1e-9);
|
||||
|
||||
// Backward right turn
|
||||
drive.curvatureDrive(-0.5, 0.5, false);
|
||||
assertEquals(-0.25, left.get(), 1e-9);
|
||||
assertEquals(-0.75, right.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testCurvatureDriveTurnInPlace() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var drive = new DifferentialDrive(left, right);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.curvatureDrive(1.0, 0.0, true);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward left turn
|
||||
drive.curvatureDrive(0.5, -0.5, true);
|
||||
assertEquals(0.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward right turn
|
||||
drive.curvatureDrive(0.5, 0.5, true);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(0.0, right.get(), 1e-9);
|
||||
|
||||
// Backward
|
||||
drive.curvatureDrive(-1.0, 0.0, true);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward left turn
|
||||
drive.curvatureDrive(-0.5, -0.5, true);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(0.0, right.get(), 1e-9);
|
||||
|
||||
// Backward right turn
|
||||
drive.curvatureDrive(-0.5, 0.5, true);
|
||||
assertEquals(0.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTankDrive() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var drive = new DifferentialDrive(left, right);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.tankDrive(1.0, 1.0, false);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward left turn
|
||||
drive.tankDrive(0.5, 1.0, false);
|
||||
assertEquals(0.5, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward right turn
|
||||
drive.tankDrive(1.0, 0.5, false);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(0.5, right.get(), 1e-9);
|
||||
|
||||
// Backward
|
||||
drive.tankDrive(-1.0, -1.0, false);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward left turn
|
||||
drive.tankDrive(-0.5, -1.0, false);
|
||||
assertEquals(-0.5, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward right turn
|
||||
drive.tankDrive(-0.5, 1.0, false);
|
||||
assertEquals(-0.5, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTankDriveSquared() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var drive = new DifferentialDrive(left, right);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.tankDrive(1.0, 1.0, true);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward left turn
|
||||
drive.tankDrive(0.5, 1.0, true);
|
||||
assertEquals(0.25, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
|
||||
// Forward right turn
|
||||
drive.tankDrive(1.0, 0.5, true);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(0.25, right.get(), 1e-9);
|
||||
|
||||
// Backward
|
||||
drive.tankDrive(-1.0, -1.0, true);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward left turn
|
||||
drive.tankDrive(-0.5, -1.0, true);
|
||||
assertEquals(-0.25, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
|
||||
// Backward right turn
|
||||
drive.tankDrive(-1.0, -0.5, true);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-0.25, right.get(), 1e-9);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.drive;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class KilloughDriveTest {
|
||||
@Test
|
||||
void testCartesian() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var back = new MockMotorController();
|
||||
var drive = new KilloughDrive(left, right, back);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.driveCartesian(1.0, 0.0, 0.0);
|
||||
assertEquals(0.5, left.get(), 1e-9);
|
||||
assertEquals(-0.5, right.get(), 1e-9);
|
||||
assertEquals(0.0, back.get(), 1e-9);
|
||||
|
||||
// Left
|
||||
drive.driveCartesian(0.0, -1.0, 0.0);
|
||||
assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9);
|
||||
assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9);
|
||||
assertEquals(1.0, back.get(), 1e-9);
|
||||
|
||||
// Right
|
||||
drive.driveCartesian(0.0, 1.0, 0.0);
|
||||
assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9);
|
||||
assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9);
|
||||
assertEquals(-1.0, back.get(), 1e-9);
|
||||
|
||||
// Rotate CCW
|
||||
drive.driveCartesian(0.0, 0.0, -1.0);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
assertEquals(-1.0, back.get(), 1e-9);
|
||||
|
||||
// Rotate CW
|
||||
drive.driveCartesian(0.0, 0.0, 1.0);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
assertEquals(1.0, back.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testCartesiangyro90CW() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var back = new MockMotorController();
|
||||
var drive = new KilloughDrive(left, right, back);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward in global frame; left in robot frame
|
||||
drive.driveCartesian(1.0, 0.0, 0.0, 90.0);
|
||||
assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9);
|
||||
assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9);
|
||||
assertEquals(1.0, back.get(), 1e-9);
|
||||
|
||||
// Left in global frame; backward in robot frame
|
||||
drive.driveCartesian(0.0, -1.0, 0.0, 90.0);
|
||||
assertEquals(-0.5, left.get(), 1e-9);
|
||||
assertEquals(0.5, right.get(), 1e-9);
|
||||
assertEquals(0.0, back.get(), 1e-9);
|
||||
|
||||
// Right in global frame; forward in robot frame
|
||||
drive.driveCartesian(0.0, 1.0, 0.0, 90.0);
|
||||
assertEquals(0.5, left.get(), 1e-9);
|
||||
assertEquals(-0.5, right.get(), 1e-9);
|
||||
assertEquals(0.0, back.get(), 1e-9);
|
||||
|
||||
// Rotate CCW
|
||||
drive.driveCartesian(0.0, 0.0, -1.0, 90.0);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
assertEquals(-1.0, back.get(), 1e-9);
|
||||
|
||||
// Rotate CW
|
||||
drive.driveCartesian(0.0, 0.0, 1.0, 90.0);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
assertEquals(1.0, back.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPolar() {
|
||||
var left = new MockMotorController();
|
||||
var right = new MockMotorController();
|
||||
var back = new MockMotorController();
|
||||
var drive = new KilloughDrive(left, right, back);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.drivePolar(1.0, 0.0, 0.0);
|
||||
assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9);
|
||||
assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9);
|
||||
assertEquals(-1.0, back.get(), 1e-9);
|
||||
|
||||
// Left
|
||||
drive.drivePolar(1.0, -90.0, 0.0);
|
||||
assertEquals(-0.5, left.get(), 1e-9);
|
||||
assertEquals(0.5, right.get(), 1e-9);
|
||||
assertEquals(0.0, back.get(), 1e-9);
|
||||
|
||||
// Right
|
||||
drive.drivePolar(1.0, 90.0, 0.0);
|
||||
assertEquals(0.5, left.get(), 1e-9);
|
||||
assertEquals(-0.5, right.get(), 1e-9);
|
||||
assertEquals(0.0, back.get(), 1e-9);
|
||||
|
||||
// Rotate CCW
|
||||
drive.drivePolar(0.0, 0.0, -1.0);
|
||||
assertEquals(-1.0, left.get(), 1e-9);
|
||||
assertEquals(-1.0, right.get(), 1e-9);
|
||||
assertEquals(-1.0, back.get(), 1e-9);
|
||||
|
||||
// Rotate CW
|
||||
drive.drivePolar(0.0, 0.0, 1.0);
|
||||
assertEquals(1.0, left.get(), 1e-9);
|
||||
assertEquals(1.0, right.get(), 1e-9);
|
||||
assertEquals(1.0, back.get(), 1e-9);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,147 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.drive;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class MecanumDriveTest {
|
||||
@Test
|
||||
void testCartesian() {
|
||||
var fl = new MockMotorController();
|
||||
var fr = new MockMotorController();
|
||||
var rl = new MockMotorController();
|
||||
var rr = new MockMotorController();
|
||||
var drive = new MecanumDrive(fl, fr, rl, rr);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.driveCartesian(1.0, 0.0, 0.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Left
|
||||
drive.driveCartesian(0.0, -1.0, 0.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
|
||||
// Right
|
||||
drive.driveCartesian(0.0, 1.0, 0.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(-1.0, fr.get(), 1e-9);
|
||||
assertEquals(-1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Rotate CCW
|
||||
drive.driveCartesian(0.0, 0.0, -1.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(-1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Rotate CW
|
||||
drive.driveCartesian(0.0, 0.0, 1.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(-1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testCartesiangyro90CW() {
|
||||
var fl = new MockMotorController();
|
||||
var fr = new MockMotorController();
|
||||
var rl = new MockMotorController();
|
||||
var rr = new MockMotorController();
|
||||
var drive = new MecanumDrive(fl, fr, rl, rr);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward in global frame; left in robot frame
|
||||
drive.driveCartesian(1.0, 0.0, 0.0, 90.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
|
||||
// Left in global frame; backward in robot frame
|
||||
drive.driveCartesian(0.0, -1.0, 0.0, 90.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(-1.0, fr.get(), 1e-9);
|
||||
assertEquals(-1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
|
||||
// Right in global frame; forward in robot frame
|
||||
drive.driveCartesian(0.0, 1.0, 0.0, 90.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Rotate CCW
|
||||
drive.driveCartesian(0.0, 0.0, -1.0, 90.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(-1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Rotate CW
|
||||
drive.driveCartesian(0.0, 0.0, 1.0, 90.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(-1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPolar() {
|
||||
var fl = new MockMotorController();
|
||||
var fr = new MockMotorController();
|
||||
var rl = new MockMotorController();
|
||||
var rr = new MockMotorController();
|
||||
var drive = new MecanumDrive(fl, fr, rl, rr);
|
||||
drive.setDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.drivePolar(1.0, 0.0, 0.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Left
|
||||
drive.drivePolar(1.0, -90.0, 0.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
|
||||
// Right
|
||||
drive.drivePolar(1.0, 90.0, 0.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(-1.0, fr.get(), 1e-9);
|
||||
assertEquals(-1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Rotate CCW
|
||||
drive.drivePolar(0.0, 0.0, -1.0);
|
||||
assertEquals(-1.0, fl.get(), 1e-9);
|
||||
assertEquals(-1.0, fr.get(), 1e-9);
|
||||
assertEquals(1.0, rl.get(), 1e-9);
|
||||
assertEquals(1.0, rr.get(), 1e-9);
|
||||
|
||||
// Rotate CW
|
||||
drive.drivePolar(0.0, 0.0, 1.0);
|
||||
assertEquals(1.0, fl.get(), 1e-9);
|
||||
assertEquals(1.0, fr.get(), 1e-9);
|
||||
assertEquals(-1.0, rl.get(), 1e-9);
|
||||
assertEquals(-1.0, rr.get(), 1e-9);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user