Announcement

Collapse
No announcement yet.

Team 9970 needs help

Collapse
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • Team 9970 needs help

    Hello,
    We have been having a lot of difficultly with connection errors. We have tried replacing cables, securing cables and looking for errors in our code. Nothing we have tried has yet worked. We think the jack on the hub might be damaged. On Saturday, we tried to get the phone to talk to the robot through the connector on the other hub and we couldn't get it to work. We have sent our Log File to Rev robotics for assistance. They suggested that we also post our code on the forum to see if any others could find and error in the code. They feel it is more of a coding error than a hardware problem. I have attached our code. If anyone could look through it and see if there is anything that is causing it to throw errors that would be greatly appreciated.
    The errors that we keep getting are:
    stuckinloop
    stuckinstop
    stuckininitiatization
    nullpointerexception error

    Our first competition is on Jan 26th and we are feeling a bit panicked and frustrated. We have attached both our autonomous program and our drive program. Thank you so much for your help!!

    Stellar Pixels


    ---Code Begins Here---
    package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.eventloop.opmode.Autonomous ; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMo de; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.NormalizedColorSen sor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.SwitchableLight; import android.app.Activity; import android.graphics.Color; import android.view.View; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import org.firstinspires.ftc.robotcontroller.external.sam ples.ConceptVuforiaNavigation; import org.firstinspires.ftc.robotcore.external.ClassFact ory; import org.firstinspires.ftc.robotcore.external.matrices. OpenGLMatrix; import org.firstinspires.ftc.robotcore.external.matrices. VectorF; import org.firstinspires.ftc.robotcore.external.navigatio n.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigatio n.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigatio n.AxesReference; import org.firstinspires.ftc.robotcore.external.navigatio n.Orientation; import org.firstinspires.ftc.robotcore.external.navigatio n.RelicRecoveryVuMark; import org.firstinspires.ftc.robotcore.external.navigatio n.VuMarkInstanceId; import org.firstinspires.ftc.robotcore.external.navigatio n.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.navigatio n.VuforiaTrackable; import org.firstinspires.ftc.robotcore.external.navigatio n.VuforiaTrackableDefaultListener; import org.firstinspires.ftc.robotcore.external.navigatio n.VuforiaTrackables; /** * * * Created by Steller Pixels on 10/10/2017. */ ///we are back Fleshy mammals @Autonomous(name="Main_Autonomous_V3", group="TestGroup") public class Main_Autonomous_V3 extends LinearOpMode { //welcome to our fleshy mammal chaos DcMotor LeftMotorFront; DcMotor RightMotorFront; DcMotor LeftMotorBack; DcMotor RightMotorBack; DcMotor ArmMotor; Servo rightServo; Servo leftServo; Servo wristServo; Servo leftpincerServo; Servo rightpincerServo; Servo phoneServo; ColorSensor rightColorSensor; ColorSensor leftColorSensor; ColorSensor plateColorSensor; OpenGLMatrix lastLocation = null; VuforiaLocalizer vuforia; public void runOpMode() { int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifie r("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = "AdMdbHb/////AAAAGZib3MCHAk2csXTgBNT5//4kRyFZmvh5ac5BiirpY7ylmPAe3gbwk5lHXfr9ph+7FkTl5rt1 WJMwVZBwp3RfVE30Z2EBi1xG4HFsl3jsOiQAareJSrID5QlTiX 91QH/vs+rqAnQPAI8olOhscVhxKUuvS0AQAOdX3eJ759vwAEzrzRjUC bKRypFfjUoNXkxnV0MrZrBHOTIzhh/r9N4R+hZubt0k8jC2RCOEWynwjzTHMCBv5AUf7vPstQY0wXt5r Zrruf4OzfEKaldbZqxkWUVXlBWS0ThtceAT/efFqSvIlYRd1l4R63zlRF0SIh73TBlqeRDnmXXhXGSDPFrw/tHo9o5yG1wK8W100pKa/5aO"; parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark" ); VuforiaTrackable relicTemplate = relicTrackables.get(0); relicTemplate.setName("relicVuMarkTemplate"); LeftMotorFront = hardwareMap.dcMotor.get("LeftMotorFront"); LeftMotorBack = hardwareMap.dcMotor.get("LeftMotorBack"); RightMotorFront = hardwareMap.dcMotor.get("RightMotorFront"); RightMotorBack = hardwareMap.dcMotor.get("RightMotorBack"); ArmMotor = hardwareMap.dcMotor.get("ArmMotor"); rightServo = hardwareMap.servo.get("RightArmServo"); leftServo = hardwareMap.servo.get("LeftArmServo"); wristServo = hardwareMap.servo.get("WristServo"); leftpincerServo = hardwareMap.servo.get("LeftPincer"); rightpincerServo = hardwareMap.servo.get("RightPincer"); phoneServo = hardwareMap.servo.get("PhoneServo"); rightColorSensor = hardwareMap.colorSensor.get("RightColor"); leftColorSensor = hardwareMap.colorSensor.get("LeftColor"); plateColorSensor = hardwareMap.colorSensor.get("PlateColorSensor"); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_EN CODER); RightMotorFront.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER); LeftMotorFront.setMode(DcMotor.RunMode.STOP_AND_RE SET_ENCODER); RightMotorBack.setMode(DcMotor.RunMode.STOP_AND_RE SET_ENCODER); LeftMotorBack.setMode(DcMotor.RunMode.STOP_AND_RES ET_ENCODER); LeftMotorFront.setDirection(DcMotor.Direction.REVE RSE); LeftMotorBack.setDirection(DcMotor.Direction.REVER SE); leftpincerServo.setPosition(.5); rightpincerServo.setPosition(.5); rightServo.setPosition(.15); leftServo.setPosition(.69); wristServo.setPosition(.05); rightColorSensor.enableLed(true); leftColorSensor.enableLed(true); plateColorSensor.enableLed(true); telemetry.addData("RightBlue", rightColorSensor.blue()); telemetry.addData("RightRed", rightColorSensor.red()); telemetry.addData("PlateColorSensorBlue", plateColorSensor.blue()); telemetry.addData("PlateColorSensorRed", plateColorSensor.red()); waitForStart(); relicTrackables.activate(); while (opModeIsActive()) { RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); if (vuMark != RelicRecoveryVuMark.UNKNOWN) { telemetry.addData("VuMark", "%s visible", vuMark); } telemetry.update(); if (plateColorSensor.red() < 20) { leftServo.setPosition(0.15); phoneServo.setPosition(1); sleep(1500); LeftArmJewelBlue(); sleep(1000); if (vuMark == RelicRecoveryVuMark.CENTER) { RightMotorFront.setTargetPosition(3982); LeftMotorFront.setTargetPosition(3982); RightMotorBack.setTargetPosition(3982); LeftMotorBack.setTargetPosition(3982); } if (vuMark == RelicRecoveryVuMark.RIGHT) { RightMotorFront.setTargetPosition(4977); LeftMotorFront.setTargetPosition(4977); RightMotorBack.setTargetPosition(4977); LeftMotorBack.setTargetPosition(4977); } if (vuMark == RelicRecoveryVuMark.LEFT || vuMark == RelicRecoveryVuMark.UNKNOWN) { RightMotorFront.setTargetPosition(2986); LeftMotorFront.setTargetPosition(2986); RightMotorBack.setTargetPosition(2986); LeftMotorBack.setTargetPosition(2986); } } else { rightServo.setPosition(.7); phoneServo.setPosition(.5); sleep(1500); RightArmJewelRed(); sleep(1000); if (vuMark == RelicRecoveryVuMark.CENTER) { RightMotorFront.setTargetPosition(3982); LeftMotorFront.setTargetPosition(3982); RightMotorBack.setTargetPosition(3982); LeftMotorBack.setTargetPosition(3982); } if (vuMark == RelicRecoveryVuMark.LEFT) { RightMotorFront.setTargetPosition(4977); LeftMotorFront.setTargetPosition(4977); RightMotorBack.setTargetPosition(4977); LeftMotorBack.setTargetPosition(4977); } if (vuMark == RelicRecoveryVuMark.RIGHT || vuMark == RelicRecoveryVuMark.UNKNOWN) { RightMotorFront.setTargetPosition(2986); LeftMotorFront.setTargetPosition(2986); RightMotorBack.setTargetPosition(2986); LeftMotorBack.setTargetPosition(2986); } } } } public void RightArmJewelRed() { // wait until arms are in position or lapsed time ElapsedTime settime = new ElapsedTime(); while (rightColorSensor.blue() == 0 && rightColorSensor.red() == 0 && settime.milliseconds() < 1000) { } if (rightColorSensor.red() > 1) { RightMotorFront.setPower(.5); RightMotorBack.setPower(.5); LeftMotorFront.setPower(.5); LeftMotorBack.setPower(.5); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); } else if (rightColorSensor.blue() > 1) { RightMotorFront.setPower(-.5); RightMotorBack.setPower(-.5); LeftMotorFront.setPower(-.5); LeftMotorBack.setPower(-.5); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); } rightServo.setPosition(.15); sleep(1500); } public void LeftArmJewelBlue() { ElapsedTime settime = new ElapsedTime(); while (rightColorSensor.blue() == 0 && rightColorSensor.red() == 0 && settime.milliseconds() < 1000) { } if (leftColorSensor.blue() > 1) { RightMotorFront.setPower(.5); RightMotorBack.setPower(.5); LeftMotorFront.setPower(.5); LeftMotorBack.setPower(.5); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); } else if (leftColorSensor.red() > 1) ;{ RightMotorFront.setPower(-.5); RightMotorBack.setPower(-.5); LeftMotorFront.setPower(-.5); LeftMotorBack.setPower(-.5); leftServo.setPosition(.69); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); leftServo.setPosition(.69); } leftServo.setPosition(.69); sleep(1500); } } ---Code Ends Here--- Here is our drive program: ---Code Begins Here---
    package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; import java.lang.Object; /** * Created by Steller Pixels on 1/2/2018. */ @TeleOp(name="Mechnum_V3", group="TestGroup") public class Mechnum_Drive_V3 extends OpMode { //HELLO!!! FLESHY MAMMALSSS!!!!!!! DcMotor LeftMotorFront; DcMotor RightMotorFront; DcMotor LeftMotorBack; DcMotor RightMotorBack; DcMotor ArmMotor; Servo rightServo; Servo leftServo; Servo wristServo; Servo leftpincerServo; Servo rightpincerServo; Servo phoneServo; ColorSensor rightColorSensor; ColorSensor leftColorSensor; ColorSensor plateColorSensor; boolean buttonIsReleased = true; int counter; double xIn; double yIn; double rotation; double motorPosition; double WheelSpeed[] = new double[4]; @Override public void init(){ LeftMotorFront = hardwareMap.dcMotor.get("LeftMotorFront"); LeftMotorBack = hardwareMap.dcMotor.get("LeftMotorBack"); RightMotorFront = hardwareMap.dcMotor.get("RightMotorFront"); RightMotorBack = hardwareMap.dcMotor.get("RightMotorBack"); ArmMotor = hardwareMap.dcMotor.get("ArmMotor"); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_EN CODER); ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); counter = 0; rightServo = hardwareMap.servo.get("RightArmServo"); leftServo = hardwareMap.servo.get("LeftArmServo"); wristServo = hardwareMap.servo.get("WristServo"); leftpincerServo = hardwareMap.servo.get("LeftPincer"); rightpincerServo = hardwareMap.servo.get("RightPincer"); phoneServo = hardwareMap.servo.get("PhoneServo"); rightColorSensor = hardwareMap.colorSensor.get("RightColor"); leftColorSensor = hardwareMap.colorSensor.get("LeftColor"); plateColorSensor = hardwareMap.colorSensor.get("PlateColorSensor"); LeftMotorFront.setDirection(DcMotor.Direction.REVE RSE); LeftMotorBack.setDirection(DcMotor.Direction.REVER SE); rightpincerServo.setPosition(.5); leftpincerServo.setPosition(.5); rightServo.setPosition(.15); leftServo.setPosition(.69); wristServo.setPosition(.05); phoneServo.setPosition(.3); } @Override //runOpMode public void loop() { motorPosition = ArmMotor.getCurrentPosition(); xIn = gamepad1.left_stick_x; yIn = gamepad1.left_stick_y; rotation = gamepad1.right_stick_x; WheelSpeed[0] = xIn + yIn + rotation; WheelSpeed[1] = -xIn + yIn - rotation; WheelSpeed[2] = -xIn + yIn + rotation; WheelSpeed[3] = xIn + yIn - rotation; normalize(WheelSpeed); LeftMotorFront.setPower(WheelSpeed[0]); RightMotorFront.setPower(WheelSpeed[1]); LeftMotorBack.setPower(WheelSpeed[2]); RightMotorBack.setPower(WheelSpeed[3]); rightColorSensor.enableLed(true); leftColorSensor.enableLed(true); plateColorSensor.enableLed(true); telemetry.addData("wrist position" , wristServo.getPosition()); telemetry.addData("Motor start Position", ArmMotor.getTargetPosition()); telemetry.addData("Current Motor Position", ArmMotor.getCurrentPosition()); telemetry.addData("Blue", rightColorSensor.blue()); telemetry.addData("Red ", rightColorSensor.red()); //pincer thing if (gamepad1.y) { //closed leftpincerServo.setPosition(.45); rightpincerServo.setPosition(.4); } if (gamepad1.x) { //open leftpincerServo.setPosition(.7); rightpincerServo.setPosition(.2) ; } //right servo if (gamepad1.a) { rightServo.setPosition(.3); } else { rightServo.setPosition(.15); } //left servo if (gamepad1.b) { leftServo.setPosition(0.3); } else { leftServo.setPosition(.69); } //Motor arm up and down if (counter == 0) { ArmMotor.setTargetPosition(0); ArmMotor.setPower(-5); wristServo.setPosition(.41); } if (counter == 1) { ArmMotor.setTargetPosition(575); ArmMotor.setPower(.5); wristServo.setPosition(.65); } if (counter == 2) { ArmMotor.setTargetPosition(1150 ); ArmMotor.setPower(.5); wristServo.setPosition(.7); } if (counter == 3) { ArmMotor.setTargetPosition(1722); ArmMotor.setPower(.5); wristServo.setPosition(.79); } if (gamepad1.right_bumper){ if (buttonIsReleased) { buttonIsReleased = false; counter++; } } else { buttonIsReleased = true; } if (counter == 4) { counter = 0; } /* if (gamepad1.right_bumper) { ArmMotor.setPower(-.5); } else { ArmMotor.setPower(0); } if (gamepad1.right_trigger > .2) { ArmMotor.setPower(.5); } else { ArmMotor.setPower(0); } */ } protected static void normalize(double WheelSpeed[]) { double maxMagnitude = Math.abs(WheelSpeed[0]); int i; for (i = 1; i < 4; i++){ double temp = Math.abs(WheelSpeed[i]); if (maxMagnitude < temp) { maxMagnitude = temp; } } if (maxMagnitude > 1.0) { for (i = 0; i < 4; i++) { WheelSpeed[i] = WheelSpeed[i] / maxMagnitude; } } } } ---Code Ends Here---

  • #2
    Can you paste your code in notepad and then copy paste from notepad to here with code tag ? You will get better help by improving readability

    Comment


    • #3
      Code:
      package org.firstinspires.ftc.teamcode;
      // various imports
      /** * * * Created by Steller Pixels on 10/10/2017. */
      ///we are back Fleshy mammals
      @Autonomous(name="Main_Autonomous_V3", group="TestGroup")
      public class Main_Autonomous_V3 extends LinearOpMode {
          //welcome to our fleshy mammal chaos
      DcMotor LeftMotorFront; DcMotor RightMotorFront; DcMotor LeftMotorBack; DcMotor RightMotorBack; DcMotor ArmMotor; Servo rightServo; Servo leftServo; Servo wristServo; Servo leftpincerServo; Servo rightpincerServo; Servo phoneServo; ColorSensor rightColorSensor; ColorSensor leftColorSensor; ColorSensor plateColorSensor; OpenGLMatrix lastLocation = null; VuforiaLocalizer vuforia; public void runOpMode() { int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifie r("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = "AdMdbHb/////AAAAGZib3MCHAk2csXTgBNT5//4kRyFZmvh5ac5BiirpY7ylmPAe3gbwk5lHXfr9ph+7FkTl5rt1 WJMwVZBwp3RfVE30Z2EBi1xG4HFsl3jsOiQAareJSrID5QlTiX 91QH/vs+rqAnQPAI8olOhscVhxKUuvS0AQAOdX3eJ759vwAEzrzRjUC bKRypFfjUoNXkxnV0MrZrBHOTIzhh/r9N4R+hZubt0k8jC2RCOEWynwjzTHMCBv5AUf7vPstQY0wXt5r Zrruf4OzfEKaldbZqxkWUVXlBWS0ThtceAT/efFqSvIlYRd1l4R63zlRF0SIh73TBlqeRDnmXXhXGSDPFrw/tHo9o5yG1wK8W100pKa/5aO"; parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark" ); VuforiaTrackable relicTemplate = relicTrackables.get(0); relicTemplate.setName("relicVuMarkTemplate"); LeftMotorFront = hardwareMap.dcMotor.get("LeftMotorFront"); LeftMotorBack = hardwareMap.dcMotor.get("LeftMotorBack"); RightMotorFront = hardwareMap.dcMotor.get("RightMotorFront"); RightMotorBack = hardwareMap.dcMotor.get("RightMotorBack"); ArmMotor = hardwareMap.dcMotor.get("ArmMotor"); rightServo = hardwareMap.servo.get("RightArmServo"); leftServo = hardwareMap.servo.get("LeftArmServo"); wristServo = hardwareMap.servo.get("WristServo"); leftpincerServo = hardwareMap.servo.get("LeftPincer"); rightpincerServo = hardwareMap.servo.get("RightPincer"); phoneServo = hardwareMap.servo.get("PhoneServo"); rightColorSensor = hardwareMap.colorSensor.get("RightColor"); leftColorSensor = hardwareMap.colorSensor.get("LeftColor"); plateColorSensor = hardwareMap.colorSensor.get("PlateColorSensor"); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_EN CODER); RightMotorFront.setMode(DcMotor.RunMode.STOP_AND_R ESET_ENCODER); LeftMotorFront.setMode(DcMotor.RunMode.STOP_AND_RE SET_ENCODER); RightMotorBack.setMode(DcMotor.RunMode.STOP_AND_RE SET_ENCODER); LeftMotorBack.setMode(DcMotor.RunMode.STOP_AND_RES ET_ENCODER); LeftMotorFront.setDirection(DcMotor.Direction.REVE RSE); LeftMotorBack.setDirection(DcMotor.Direction.REVER SE); leftpincerServo.setPosition(.5); rightpincerServo.setPosition(.5); rightServo.setPosition(.15); leftServo.setPosition(.69); wristServo.setPosition(.05); rightColorSensor.enableLed(true); leftColorSensor.enableLed(true); plateColorSensor.enableLed(true); telemetry.addData("RightBlue", rightColorSensor.blue()); telemetry.addData("RightRed", rightColorSensor.red()); telemetry.addData("PlateColorSensorBlue", plateColorSensor.blue()); telemetry.addData("PlateColorSensorRed", plateColorSensor.red()); waitForStart(); relicTrackables.activate(); while (opModeIsActive()) { RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); if (vuMark != RelicRecoveryVuMark.UNKNOWN) { telemetry.addData("VuMark", "%s visible", vuMark); } telemetry.update(); if (plateColorSensor.red() < 20) { leftServo.setPosition(0.15); phoneServo.setPosition(1); sleep(1500); LeftArmJewelBlue(); sleep(1000); if (vuMark == RelicRecoveryVuMark.CENTER) { RightMotorFront.setTargetPosition(3982); LeftMotorFront.setTargetPosition(3982); RightMotorBack.setTargetPosition(3982); LeftMotorBack.setTargetPosition(3982); } if (vuMark == RelicRecoveryVuMark.RIGHT) { RightMotorFront.setTargetPosition(4977); LeftMotorFront.setTargetPosition(4977); RightMotorBack.setTargetPosition(4977); LeftMotorBack.setTargetPosition(4977); } if (vuMark == RelicRecoveryVuMark.LEFT || vuMark == RelicRecoveryVuMark.UNKNOWN) { RightMotorFront.setTargetPosition(2986); LeftMotorFront.setTargetPosition(2986); RightMotorBack.setTargetPosition(2986); LeftMotorBack.setTargetPosition(2986); } } else { rightServo.setPosition(.7); phoneServo.setPosition(.5); sleep(1500); RightArmJewelRed(); sleep(1000); if (vuMark == RelicRecoveryVuMark.CENTER) { RightMotorFront.setTargetPosition(3982); LeftMotorFront.setTargetPosition(3982); RightMotorBack.setTargetPosition(3982); LeftMotorBack.setTargetPosition(3982); } if (vuMark == RelicRecoveryVuMark.LEFT) { RightMotorFront.setTargetPosition(4977); LeftMotorFront.setTargetPosition(4977); RightMotorBack.setTargetPosition(4977); LeftMotorBack.setTargetPosition(4977); } if (vuMark == RelicRecoveryVuMark.RIGHT || vuMark == RelicRecoveryVuMark.UNKNOWN) { RightMotorFront.setTargetPosition(2986); LeftMotorFront.setTargetPosition(2986); RightMotorBack.setTargetPosition(2986); LeftMotorBack.setTargetPosition(2986); } } } } public void RightArmJewelRed() { // wait until arms are in position or lapsed time ElapsedTime settime = new ElapsedTime(); while (rightColorSensor.blue() == 0 && rightColorSensor.red() == 0 && settime.milliseconds() < 1000) { } if (rightColorSensor.red() > 1) { RightMotorFront.setPower(.5); RightMotorBack.setPower(.5); LeftMotorFront.setPower(.5); LeftMotorBack.setPower(.5); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); } else if (rightColorSensor.blue() > 1) { RightMotorFront.setPower(-.5); RightMotorBack.setPower(-.5); LeftMotorFront.setPower(-.5); LeftMotorBack.setPower(-.5); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); } rightServo.setPosition(.15); sleep(1500); } public void LeftArmJewelBlue() { ElapsedTime settime = new ElapsedTime(); while (rightColorSensor.blue() == 0 && rightColorSensor.red() == 0 && settime.milliseconds() < 1000) { } if (leftColorSensor.blue() > 1) { RightMotorFront.setPower(.5); RightMotorBack.setPower(.5); LeftMotorFront.setPower(.5); LeftMotorBack.setPower(.5); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); } else if (leftColorSensor.red() > 1) ;{ RightMotorFront.setPower(-.5); RightMotorBack.setPower(-.5); LeftMotorFront.setPower(-.5); LeftMotorBack.setPower(-.5); leftServo.setPosition(.69); sleep(100); RightMotorFront.setPower(0); RightMotorBack.setPower(0); LeftMotorFront.setPower(0); LeftMotorBack.setPower(0); leftServo.setPosition(.69); } leftServo.setPosition(.69); sleep(1500); } } ---Code Ends Here--- Here is our drive program: ---Code Begins Here---
      package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; import java.lang.Object; /** * Created by Steller Pixels on 1/2/2018. */ @TeleOp(name="Mechnum_V3", group="TestGroup") public class Mechnum_Drive_V3 extends OpMode { //HELLO!!! FLESHY MAMMALSSS!!!!!!! DcMotor LeftMotorFront; DcMotor RightMotorFront; DcMotor LeftMotorBack; DcMotor RightMotorBack; DcMotor ArmMotor; Servo rightServo; Servo leftServo; Servo wristServo; Servo leftpincerServo; Servo rightpincerServo; Servo phoneServo; ColorSensor rightColorSensor; ColorSensor leftColorSensor; ColorSensor plateColorSensor; boolean buttonIsReleased = true; int counter; double xIn; double yIn; double rotation; double motorPosition; double WheelSpeed[] = new double[4]; @Override public void init(){ LeftMotorFront = hardwareMap.dcMotor.get("LeftMotorFront"); LeftMotorBack = hardwareMap.dcMotor.get("LeftMotorBack"); RightMotorFront = hardwareMap.dcMotor.get("RightMotorFront"); RightMotorBack = hardwareMap.dcMotor.get("RightMotorBack"); ArmMotor = hardwareMap.dcMotor.get("ArmMotor"); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_EN CODER); ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); counter = 0; rightServo = hardwareMap.servo.get("RightArmServo"); leftServo = hardwareMap.servo.get("LeftArmServo"); wristServo = hardwareMap.servo.get("WristServo"); leftpincerServo = hardwareMap.servo.get("LeftPincer"); rightpincerServo = hardwareMap.servo.get("RightPincer"); phoneServo = hardwareMap.servo.get("PhoneServo"); rightColorSensor = hardwareMap.colorSensor.get("RightColor"); leftColorSensor = hardwareMap.colorSensor.get("LeftColor"); plateColorSensor = hardwareMap.colorSensor.get("PlateColorSensor"); LeftMotorFront.setDirection(DcMotor.Direction.REVE RSE); LeftMotorBack.setDirection(DcMotor.Direction.REVER SE); rightpincerServo.setPosition(.5); leftpincerServo.setPosition(.5); rightServo.setPosition(.15); leftServo.setPosition(.69); wristServo.setPosition(.05); phoneServo.setPosition(.3); } @Override //runOpMode public void loop() { motorPosition = ArmMotor.getCurrentPosition(); xIn = gamepad1.left_stick_x; yIn = gamepad1.left_stick_y; rotation = gamepad1.right_stick_x; WheelSpeed[0] = xIn + yIn + rotation; WheelSpeed[1] = -xIn + yIn - rotation; WheelSpeed[2] = -xIn + yIn + rotation; WheelSpeed[3] = xIn + yIn - rotation; normalize(WheelSpeed); LeftMotorFront.setPower(WheelSpeed[0]); RightMotorFront.setPower(WheelSpeed[1]); LeftMotorBack.setPower(WheelSpeed[2]); RightMotorBack.setPower(WheelSpeed[3]); rightColorSensor.enableLed(true); leftColorSensor.enableLed(true); plateColorSensor.enableLed(true); telemetry.addData("wrist position" , wristServo.getPosition()); telemetry.addData("Motor start Position", ArmMotor.getTargetPosition()); telemetry.addData("Current Motor Position", ArmMotor.getCurrentPosition()); telemetry.addData("Blue", rightColorSensor.blue()); telemetry.addData("Red ", rightColorSensor.red()); //pincer thing if (gamepad1.y) { //closed leftpincerServo.setPosition(.45); rightpincerServo.setPosition(.4); } if (gamepad1.x) { //open leftpincerServo.setPosition(.7); rightpincerServo.setPosition(.2) ; } //right servo if (gamepad1.a) { rightServo.setPosition(.3); } else { rightServo.setPosition(.15); } //left servo if (gamepad1.b) { leftServo.setPosition(0.3); } else { leftServo.setPosition(.69); } //Motor arm up and down if (counter == 0) { ArmMotor.setTargetPosition(0); ArmMotor.setPower(-5); wristServo.setPosition(.41); } if (counter == 1) { ArmMotor.setTargetPosition(575); ArmMotor.setPower(.5); wristServo.setPosition(.65); } if (counter == 2) { ArmMotor.setTargetPosition(1150 ); ArmMotor.setPower(.5); wristServo.setPosition(.7); } if (counter == 3) { ArmMotor.setTargetPosition(1722); ArmMotor.setPower(.5); wristServo.setPosition(.79); } if (gamepad1.right_bumper){ if (buttonIsReleased) { buttonIsReleased = false; counter++; } } else { buttonIsReleased = true; } if (counter == 4) { counter = 0; } /* if (gamepad1.right_bumper) { ArmMotor.setPower(-.5); } else { ArmMotor.setPower(0); } if (gamepad1.right_trigger > .2) { ArmMotor.setPower(.5); } else { ArmMotor.setPower(0); } */ } protected static void normalize(double WheelSpeed[]) { double maxMagnitude = Math.abs(WheelSpeed[0]); int i; for (i = 1; i < 4; i++){ double temp = Math.abs(WheelSpeed[i]); if (maxMagnitude < temp) { maxMagnitude = temp; } } if (maxMagnitude > 1.0) { for (i = 0; i < 4; i++) { WheelSpeed[i] = WheelSpeed[i] / maxMagnitude; } } } }
      Mentor, teams 8578 & 11959

      Comment


      • #4
        Darn, was trying to reformat the code, but hit tab at the wrong time and it posted. Can't edit it. Sorry.
        Mentor, teams 8578 & 11959

        Comment

        Working...
        X