Skip to content

Commit

Permalink
Fix movement for the new wheels
Browse files Browse the repository at this point in the history
  • Loading branch information
Sergiu Marton committed Jan 19, 2019
1 parent c2f08a4 commit 7fabea4
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 73 deletions.
67 changes: 23 additions & 44 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roverruckus/AutoCrater.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,58 +187,58 @@ class AutoCrater : LinearOpMode() {
1 -> {
goForwardOnAngle(time = 0.7, power=0.45, angle=-135.0)
waitForSeconds(0.75)
goForwardOnAngle(time = 0.7, power=-0.45, angle=-135.0)
goForwardOnAngle(time = 0.65, power=-0.45, angle=-135.0)
rotate(degrees = -(180.0 - 55.0), power = 0.34)
goForwardOnAngle(time = 1.65, power = -0.35, angle = 90.0)

// ============================
waitForSeconds(0.2)
rotate(degrees = 45.0, power = 0.34)

goForwardOnAngle(time = 1.35, power = -0.45, angle = 145.0)
goForwardOnAngle(time = 1.52, power = -0.45, angle = 145.0)
dropTeamMarker()
waitForSeconds(0.5)
goForwardOnAngle(time = 0.1, power = 0.7, angle = 142.0)
goForwardOnAngle(time = 1.15, power = 0.5, angle = 142.0)
goForwardOnAngle(time = 1.2, power = 0.5, angle = 145.0)
goForwardOnAngle(time = 0.1, power = 0.6, angle = 142.0)
goForwardOnAngle(time = 0.8, power = 0.8, angle = 142.0)
goForwardOnAngle(time = 0.6, power = 0.8, angle = 145.0)
letDownArm()
}
2 -> {
goForwardOnAngle(time = 0.7, power = 0.45, angle = 180.0)
waitForSeconds(0.7)
goForwardOnAngle(time = 0.7, power = -0.45, angle = 180.0)
waitForSeconds(0.65)
goForwardOnAngle(time = 0.62, power = -0.45, angle = 180.0)
rotate(degrees = -90.0, power = 0.34)
goForwardOnAngle(time = 1.6, power = -0.45, angle = 90.0)
goForwardOnAngle(time = 1.65, power = -0.45, angle = 90.0)

// ============================
waitForSeconds(0.2)
rotate(degrees = 45.0, power = 0.34)

goForwardOnAngle(time = 1.35, power = -0.45, angle = 143.0)
goForwardOnAngle(time = 1.45, power = -0.45, angle = 143.0)
dropTeamMarker()
waitForSeconds(0.5)
goForwardOnAngle(time = 0.2, power = 0.8, angle = 142.0)
goForwardOnAngle(time = 1.1, power = 0.5, angle = 142.0)
goForwardOnAngle(time = 1.0, power = 0.5, angle = 147.0)
goForwardOnAngle(time = 0.6, power = 0.8, angle = 142.0)
goForwardOnAngle(time = 0.55, power = 0.8, angle = 147.0)
letDownArm()
}
3 -> {
goForwardOnAngle(time = 0.7, power = 0.45, angle = 145.0)
waitForSeconds(0.75)
goForwardOnAngle(time = 0.7, power = -0.45, angle = 147.0)
goForwardOnAngle(time = 0.65, power = -0.45, angle = 147.0)
rotate(degrees = -55.0, power = 0.34)
goForwardOnAngle(time = 1.8, power = -0.35, angle = 90.0)
goForwardOnAngle(time = 2.1, power = -0.35, angle = 90.0)

// ============================
waitForSeconds(0.2)
rotate(degrees = 45.0, power = 0.34)

goForwardOnAngle(time = 1.35, power = -0.45, angle = 145.0)
goForwardOnAngle(time = 1.4, power = -0.45, angle = 145.0)
dropTeamMarker()
waitForSeconds(0.5)
goForwardOnAngle(time = 0.1, power = 0.7, angle = 142.0)
goForwardOnAngle(time = 1.13, power = 0.5, angle = 142.0)
goForwardOnAngle(time = 1.2, power = 0.5, angle = 149.0)
goForwardOnAngle(time = 0.7, power = 0.8, angle = 142.0)
goForwardOnAngle(time = 0.6, power = 0.8, angle = 149.0)
letDownArm()
}
}
Expand Down Expand Up @@ -315,7 +315,6 @@ class AutoCrater : LinearOpMode() {
elapsedTime.reset()

while (elapsedTime.seconds() < seconds && opModeIsActive()) {
// WAIT 0.75 secs
}
}

Expand Down Expand Up @@ -355,18 +354,20 @@ class AutoCrater : LinearOpMode() {
}

fun goForwardOnAngle(time: Double, power: Double, angle: Double) {
elapsedTime.reset()
val leftLimiter: (Double) -> Boolean
val rightLimiter: (Double) -> Boolean
val higherPower = power + 0.25
val lowerPower = power - 0.25
val normalPower = power
if (angle == 180.0) {
leftLimiter = { rotation -> rotation < 0 && rotation > -177.5 }
rightLimiter = { rotation -> rotation > 0 && rotation < 177.5 }
} else {
leftLimiter = { rotation -> rotation > angle + 2.5 }
rightLimiter = { rotation -> rotation < angle - 2.5 }
}

elapsedTime.reset()
while (elapsedTime.seconds() < time && opModeIsActive()) {
val rotation = imu
.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES)
Expand All @@ -381,13 +382,16 @@ class AutoCrater : LinearOpMode() {
hardware.leftMotorPower = lowerPower
hardware.rightMotorPower = higherPower
}
else -> {
!leftLimiter(rotation) && !rightLimiter(rotation) -> {
hardware.leftMotorPower = power
hardware.rightMotorPower = power
telemetry.addData("MOVING NORMAL", "AR TREBUI SA MEARGA")
}
}
if (power < 0.0) { telemetry.addData("Move", "Backwards") }
telemetry.addData("Movement", "LM: ${hardware.leftMotorPower}; RM: ${hardware.rightMotorPower}")
telemetry.addData("Time:", "Elpsdtm: ${elapsedTime.seconds()}; time: $time")
telemetry.addData("angles", "LL: ${leftLimiter(rotation)}; RL: ${rightLimiter(rotation)}; Rot: $rotation")
telemetry.update()
}

Expand Down Expand Up @@ -459,31 +463,6 @@ class AutoCrater : LinearOpMode() {
hardware.rightMotorPower = 0.0
}


fun moveForwardOnAngle(angle: Double, time: Double) {
while (elapsedTime.seconds() < time && opModeIsActive()) {
val rotation = imu
.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES)
.firstAngle
.toDouble()
when {
rotation > angle + 5.0 -> {
hardware.leftMotorPower = 0.55
hardware.rightMotorPower = 0.35
}
rotation < angle - 5.0 -> {
hardware.leftMotorPower = 0.35
hardware.rightMotorPower = 0.55
}
else -> {
hardware.leftMotorPower = 0.4
hardware.rightMotorPower = 0.4
}
}
telemetry.update()
}
}

internal fun composeTelemetry() {

// At the beginning of each telemetry update, grab a bunch of data
Expand Down
46 changes: 17 additions & 29 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roverruckus/AutoDepot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,22 +62,15 @@ class AutoDepot : LinearOpMode() {
imu = hardwareMap.get(BNO055IMU::class.java, "imu")
imu.initialize(imuParams)

// mediaPlayer = MediaPlayer.create(hardwareMap.appContext, R.raw.bonjovi)
// mediaPlayer.setVolume(1.0f, 1.0f)
// mediaPlayer.start()

telemetry.addData("INIT", "over!")
telemetry.update()



// waitForStart()
// waitForStart()
while (!opModeIsActive() && !isStopRequested) {
telemetry.addData("Status", "Waiting in Init")
telemetry.update()
}


imu.startAccelerationIntegration(Position(), Velocity(), 1000)

composeTelemetry()
Expand Down Expand Up @@ -191,12 +184,12 @@ class AutoDepot : LinearOpMode() {
when (goldPos) {
1 -> {
goForwardOnAngle(time = 1.0, power=0.35, angle=-135.0)
goForwardOnAngle(time = 0.4, power=0.35, angle=-145.0)
goForwardOnAngle(time = 0.5, power=0.45, angle=-145.0)
}
2 -> {
goForwardOnAngle(time = 1.3, power = 0.45, angle = 180.0)
rotate(47.0, power = 0.34)
goForwardOnAngle(time = 0.15, power = 0.45, angle = 135.0)
goForwardOnAngle(time = 0.24, power = 0.65, angle = 135.0)
}
3 -> {
goForwardOnAngle(time = 0.9, power = 0.45, angle = 145.0)
Expand All @@ -218,16 +211,14 @@ class AutoDepot : LinearOpMode() {
// ===================================

when (goldPos) {
3 -> {
rotate(degrees = -80.0, power = 0.34)
goForwardOnAngle(time = 1.27, power = -0.45, angle = 45.0)
rotate(-80.0, power = 0.34)
1-> {
rotate(degrees = 85.0, power = 0.34)
goForwardOnAngle(time = 1.0, power = -0.45, angle = -42.0)
dropTeamMarker()
letDownArm()
goForwardOnAngle(time = 0.6, power = 0.50, angle = -40.0)
goForwardOnAngle(time = 0.6, power = 0.55, angle = -40.0)
liftIntake()
goForwardOnAngle(time = 0.75, power = 0.50, angle = -37.0)
goForwardOnAngle(time = 0.65, power = 0.50, angle = -40.0)
goForwardOnAngle(time = 1.1, power = 0.55, angle = -43.0)
letDownArm()
}
2 -> {
rotate(degrees = 70.0, power = 0.34)
Expand All @@ -237,14 +228,16 @@ class AutoDepot : LinearOpMode() {
liftIntake()
goForwardOnAngle(time = 1.35, power = 0.55, angle = -40.0)
}
1-> {
rotate(degrees = 85.0, power = 0.34)
goForwardOnAngle(time = 1.0, power = -0.45, angle = -42.0)
3 -> {
rotate(degrees = -80.0, power = 0.34)
goForwardOnAngle(time = 1.27, power = -0.55, angle = 45.0)
rotate(-80.0, power = 0.34)
dropTeamMarker()
goForwardOnAngle(time = 0.6, power = 0.55, angle = -40.0)
liftIntake()
goForwardOnAngle(time = 1.1, power = 0.55, angle = -43.0)
letDownArm()
goForwardOnAngle(time = 0.6, power = 0.50, angle = -40.0)
liftIntake()
goForwardOnAngle(time = 0.75, power = 0.50, angle = -37.0)
goForwardOnAngle(time = 0.65, power = 0.50, angle = -32.0)
}
}

Expand All @@ -253,11 +246,6 @@ class AutoDepot : LinearOpMode() {
* ==== STOP ====
* ==============
* */


// if (isStopRequested) {
// mediaPlayer.stop()
// }
}

private fun liftArm() {
Expand Down

0 comments on commit 7fabea4

Please sign in to comment.