API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 13th, 2019
152
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.76 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2. import com.vuforia.Vuforia;
  3. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  4. import org.firstinspires.ftc.robotcore.external.tfod.TfodBase;
  5. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  6. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  7. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  8. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  9. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  10. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  11. import com.qualcomm.robotcore.hardware.DcMotor;
  12. import com.qualcomm.robotcore.util.ElapsedTime;
  13. import com.qualcomm.robotcore.util.Range;
  14. import com.qualcomm.hardware.bosch.BNO055IMU;
  15. import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
  16. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  17. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  18. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  19. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  20. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  21. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  22. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
  23. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  24. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  25. import java.util.List;
  26.  
  27. @Autonomous(name="TFGYROENC1",group= "LinearOpMode")
  28.  
  29. public class Autonomocontfor extends LinearOpMode {
  30. private static final String TFOD_MODEL_ASSET = "Skystone.tflite";
  31. private static final String LABEL_FIRST_ELEMENT = "Stone";
  32. private static final String LABEL_SECOND_ELEMENT = "Skystone";
  33. private BNO055IMU imu;
  34. private ElapsedTime runtime = new ElapsedTime();
  35. private DcMotor BackRight;
  36. private DcMotor BackLeft;
  37. private DcMotor FrontRight;
  38. private DcMotor FrontLeft;
  39. double INCHES = 0;
  40. double Kp = 0;
  41. double target;
  42. boolean zeroTo360;
  43. double current;
  44. double imuAngle=0;
  45. double THRESHOLD = 2;
  46.  
  47. private static final String VUFORIA_KEY =
  48. " AU3Trdb/////AAABmb4EgBXvH0QqqkopAIsu5GpM7KbbJoDF3A52lamSYVnQiJEpPbutvqQ2T88XPN5JrxT0AOrlVdoNz+EYMSjHMt99pLkMEXxrRHpdZKfypK14migPrrocBRMn7pe2YNRkjrIe27hIsGKN7yiFoXphsfdmIz5w7FKh0Lh1aqQPt740Chy/B3WFz/0BHLy7T2CZZN6ADwyTyw0NNBTNFvmz3XkMI1ms0Fr0Lofzmlv7qLWt6T1pwG1RqU3aHb/nW0f2D3FzcQsSxlZvuRCMPkzZ7wKfghgv2YYV3CaDqew5BAo6PViV7bGtC9xAwzHW/bDA8nXw96jBhITb7ARBX8O2jdXhU6Q2Y0T60KMxE01LXEWP ";
  49. private VuforiaLocalizer vuforia;
  50. private TFObjectDetector tfod;
  51.  
  52. int DISTANCE = (int)Math.floor(INCHES) * (int)Math.floor(COUNTS_PER_INCH);
  53. static final double PI=3.1416;
  54. static final double COUNTS_PER_MOTOR_REV = 28 ; // eg: TETRIX Motor Encoder
  55. static final double DRIVE_GEAR_REDUCTION = 50 ;
  56. static final double WHEEL_DIAMETER= 4; // This is < 1.0 if geared UP // For figuring circumference
  57. static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (PI*WHEEL_DIAMETER);
  58. double power;
  59.  
  60. @Override
  61.  
  62. public void runOpMode() {
  63.  
  64. Vuforia.init();
  65.  
  66.  
  67. if (ClassFactory.getInstance().canCreateTFObjectDetector())
  68. {
  69. initTfod();
  70.  
  71. }
  72. else
  73. {
  74. telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  75. }
  76.  
  77. if (tfod != null) {
  78. tfod.activate();
  79. }
  80. BNO055IMU.Parameters imuParameters;
  81. Orientation angles;
  82. telemetry.addData("Status", "Initialized");
  83. telemetry.update();
  84.  
  85. imu = hardwareMap.get(BNO055IMU.class, "imu");
  86. BackLeft = hardwareMap.dcMotor.get("BackLeft");
  87. BackRight = hardwareMap.dcMotor.get("BackRight");
  88. FrontLeft = hardwareMap.dcMotor.get("FrontLeft");
  89. FrontRight = hardwareMap.dcMotor.get("FrontRight");
  90.  
  91. BackRight.setDirection(DcMotorSimple.Direction.REVERSE);
  92. FrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
  93.  
  94. // Create new IMU Parameters object.
  95. imuParameters = new BNO055IMU.Parameters();
  96. // Use degrees as angle unit.
  97. imuParameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
  98. // Express acceleration as m/s^2.
  99. // Disable logging.
  100. imuParameters.loggingEnabled = false;
  101. // Initialize IMU.
  102. imu.initialize(imuParameters);
  103.  
  104. angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  105.  
  106. waitForStart();
  107.  
  108. while (opModeIsActive()&&!!isStopRequested())
  109. {
  110. if (tfod != null) {
  111. // getUpdatedRecognitions() will return null if no new information is available since
  112. // the last time that call was made.
  113. List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  114. if (updatedRecognitions != null) {
  115. telemetry.addData("# Object Detected", updatedRecognitions.size());
  116.  
  117. // step through the list of recognitions and display boundary info.
  118. int i = 0;
  119. for (Recognition recognition : updatedRecognitions) {
  120. telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
  121. telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
  122. recognition.getLeft(), recognition.getTop());
  123. telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
  124. recognition.getRight(), recognition.getBottom());
  125. }
  126. }
  127. }
  128. if (tfod != null) {
  129. tfod.shutdown();
  130. }
  131.  
  132. // Display orientation info.
  133. telemetry.addData("rot about Z", angles.firstAngle);
  134. telemetry.addData("rot about Y", angles.secondAngle);
  135. telemetry.addData("rot about X", angles.thirdAngle);
  136.  
  137. imuAngle = angles.firstAngle;
  138.  
  139. while(Math.abs(gyroRotate(25, imu.getAngularOrientation().firstAngle, false, .01)) > THRESHOLD);
  140. telemetry.update();
  141. runtime.reset();
  142. }
  143. }
  144.  
  145. public void DriveForwardDistance (double power, double INCHES){
  146.  
  147. int DISTANCE = (int)Math.floor(INCHES) * (int)Math.floor(COUNTS_PER_INCH);
  148.  
  149.  
  150.  
  151. int prev = FrontLeft.getCurrentPosition() +FrontRight.getCurrentPosition();
  152. FrontLeft.setPower(power);
  153. BackLeft.setPower(power);
  154. FrontRight.setPower(power);
  155. BackRight.setPower(power);
  156.  
  157. while(FrontLeft.getCurrentPosition() + FrontRight.getCurrentPosition() < prev + DISTANCE) ;
  158.  
  159. FrontLeft.setPower(0.0);
  160. BackLeft.setPower(0.0);
  161. FrontRight.setPower(0.0);
  162. BackRight.setPower(0.0);
  163.  
  164. telemetry.update();
  165. }
  166.  
  167. public void DriveRightDistance (double power, double INCHES){
  168.  
  169. int DISTANCE = (int)Math.floor(INCHES) * (int)Math.floor(COUNTS_PER_INCH);
  170. double DistanceComp = (DISTANCE/(Math.sin(45)));
  171.  
  172.  
  173. int prev = FrontLeft.getCurrentPosition() - FrontRight.getCurrentPosition();
  174.  
  175. FrontLeft.setPower(power*.9);
  176. BackLeft.setPower(-power);
  177. FrontRight.setPower(-power);
  178. BackRight.setPower(power*.9);
  179.  
  180. while(FrontLeft.getCurrentPosition() - FrontRight.getCurrentPosition() < prev + DistanceComp);
  181.  
  182. FrontLeft.setPower(0.0);
  183. BackLeft.setPower(0.0);
  184. FrontRight.setPower(0.0);
  185. BackRight.setPower(0.0);
  186.  
  187. telemetry.update();
  188.  
  189. }
  190.  
  191. public void DriveLeftDistance (double power, double INCHES){
  192.  
  193. int DISTANCE = (int)Math.floor(INCHES) * (int)Math.floor(COUNTS_PER_INCH);
  194. double DistanceComp = (DISTANCE/(Math.sin(45)));
  195.  
  196.  
  197. int prev = FrontRight.getCurrentPosition() - FrontLeft.getCurrentPosition();
  198.  
  199.  
  200. FrontLeft.setPower(-power);
  201. BackLeft.setPower(power);
  202. FrontRight.setPower(power);
  203. BackRight.setPower(-power);
  204.  
  205. while(FrontRight.getCurrentPosition() - FrontLeft.getCurrentPosition() < prev + DistanceComp);
  206.  
  207. FrontLeft.setPower(0.0);
  208. BackLeft.setPower(0.0);
  209. FrontRight.setPower(0.0);
  210. BackRight.setPower(0.0);
  211.  
  212. telemetry.update();
  213.  
  214. }
  215.  
  216. public void DriveReturnDistance (double power, double INCHES){
  217.  
  218. int DISTANCE = (int)Math.floor(INCHES) * (int)Math.floor(COUNTS_PER_INCH);
  219.  
  220.  
  221.  
  222. int prev = (-FrontLeft.getCurrentPosition() -FrontRight.getCurrentPosition());
  223. FrontLeft.setPower(-power);
  224. BackLeft.setPower(-power);
  225. FrontRight.setPower(-power);
  226. BackRight.setPower(-power);
  227.  
  228. while(-FrontLeft.getCurrentPosition() - FrontRight.getCurrentPosition() < prev + DISTANCE) ;
  229.  
  230. FrontLeft.setPower(0.0);
  231. BackLeft.setPower(0.0);
  232. FrontRight.setPower(0.0);
  233. BackRight.setPower(0.0);
  234.  
  235. telemetry.update();
  236. }
  237.  
  238. public double gyroRotate(double target, double current, boolean zeroTo360, double Kp){
  239. if(zeroTo360 && current < 0)
  240. {
  241. current+= 360;
  242. }
  243.  
  244. double err = current - target;
  245. FrontRight.setPower(err*Kp);
  246. FrontLeft.setPower(-err*Kp);
  247. BackRight.setPower(err*Kp);
  248. BackLeft.setPower(-err*Kp);
  249. return err;
  250. }
  251.  
  252. private void initVuforia() {
  253. /*
  254. * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  255. */
  256. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  257.  
  258. parameters.vuforiaLicenseKey = VUFORIA_KEY;
  259. parameters.cameraDirection = CameraDirection.BACK;
  260.  
  261. // Instantiate the Vuforia engine
  262. vuforia = ClassFactory.getInstance().createVuforia(parameters);
  263.  
  264. // Loading trackables is not necessary for the TensorFlow Object Detection engine.
  265. }
  266.  
  267.  
  268.  
  269. private void initTfod() {
  270. int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  271. "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  272. TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  273. tfodParameters.minimumConfidence = 0.8;
  274. tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  275. tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
  276. }
  277.  
  278. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!