API tools faq
paste
Advertisement
Guest User

robot

a guest
Mar 20th, 2019
105
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.83 KB | None | 0 0
raw download clone embed print report
  1. /* Copyright (c) 2017 FIRST. All rights reserved.
  2. *
  3. * Redistribution and use in source and binary forms, with or without modification,
  4. * are permitted (subject to the limitations in the disclaimer below) provided that
  5. * the following conditions are met:
  6. *
  7. * Redistributions of source code must retain the above copyright notice, this list
  8. * of conditions and the following disclaimer.
  9. *
  10. * Redistributions in binary form must reproduce the above copyright notice, this
  11. * list of conditions and the following disclaimer in the documentation and/or
  12. * other materials provided with the distribution.
  13. *
  14. * Neither the name of FIRST nor the names of its contributors may be used to endorse or
  15. * promote products derived from this software without specific prior written permission.
  16. *
  17. * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
  18. * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  19. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  20. * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21. * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
  22. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  23. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  24. * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  25. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  26. * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  27. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  28. */
  29. package org.firstinspires.ftc.teamcode;
  30.  
  31. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
  32. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
  33. import com.qualcomm.hardware.motors.NeveRest40Gearmotor;
  34. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  35. import com.qualcomm.robotcore.hardware.CRServo;
  36. import com.qualcomm.robotcore.hardware.DcMotor;
  37. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  38. import com.qualcomm.robotcore.hardware.HardwareMap;
  39. import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
  40. import com.qualcomm.robotcore.hardware.Servo;
  41. import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
  42. import com.qualcomm.robotcore.util.ElapsedTime;
  43.  
  44. import android.util.Range.*;
  45.  
  46. import org.firstinspires.ftc.robotcontroller.external.samples.SensorMRRangeSensor;
  47. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  48. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  49. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  50.  
  51. /**
  52. * This is NOT an opmode.
  53. *
  54. * This class can be used to define all the specific hardware for a single robot.
  55. * In this case that robot is a Pushbot.
  56. * See PushbotTeleopTank_Iterative and others classes starting with "Pushbot" for usage examples.
  57. *
  58. * This hardware class assumes the following device names have been configured on the robot:
  59. * Note: All names are lower case and some have single spaces between words.
  60. *
  61. * Motor channel: Left drive motor: "left_drive"
  62. * Motor channel: Right drive motor: "right_drive"
  63. * Motor channel: Manipulator drive motor: "left_arm"
  64. * Servo channel: Servo to open left claw: "left_hand"
  65. * Servo channel: Servo to open right claw: "right_hand"
  66. */
  67. public class RobotBNG {
  68. /* Public OpMode members. */
  69. public DcMotor motorLeft1 = null;
  70. public DcMotor motorLeft2 = null;
  71. public DcMotor motorRight1 = null;
  72. public DcMotor motorRight2 = null;
  73.  
  74. public DcMotor sliderSucker = null;
  75. public DcMotor suckerFlip = null;
  76. public DcMotor mineralRiser = null;
  77. public DcMotor carlig = null;
  78.  
  79. public Servo MineralFlip = null;
  80. public Servo Sucker = null;
  81. public Servo marker = null;
  82.  
  83.  
  84. public int extendMotorCountsPerRev60 = 1680;
  85. public int mmPerRotationMineralRiser = 518, mmPerSuckerExtention = 600;
  86. public int extendMotorCountsPerRev40 = 1120;
  87. public int extendMotorCountsPerRev20 = 537;
  88.  
  89. // ModernRoboticsI2cRangeSensor senzorDistanta;
  90.  
  91. // IntegratingGyroscope gyro;
  92. ModernRoboticsI2cGyro gyro;
  93.  
  94. /* local OpMode members. */
  95. HardwareMap hwMap = null;
  96. private ElapsedTime period = new ElapsedTime();
  97.  
  98. /* Constructor */
  99. public RobotBNG() {
  100.  
  101. }
  102.  
  103. /* Initialize standard Hardware interfaces */
  104. public void init(HardwareMap ahwMap) {
  105. // Save reference to Hardware map
  106. hwMap = ahwMap;
  107.  
  108. // Define and Initialize Motors
  109. motorLeft1 = hwMap.get(DcMotor.class, "motorLeft1");
  110. motorLeft2 = hwMap.get(DcMotor.class, "motorLeft2");
  111. motorRight1 = hwMap.get(DcMotor.class, "motorRight1");
  112. motorRight2 = hwMap.get(DcMotor.class, "motorRight2");
  113. suckerFlip = hwMap.get(DcMotor.class, "suckerFlip");
  114. sliderSucker = hwMap.get(DcMotor.class, "sliderSucker");
  115. mineralRiser = hwMap.get(DcMotor.class, "mineralRiser");
  116. carlig = hwMap.get(DcMotor.class, "carlig");
  117.  
  118.  
  119. MineralFlip = hwMap.get(Servo.class, "MineralFlip");
  120. Sucker = hwMap.get(Servo.class, "Sucker");
  121.  
  122. // senzorDistanta = hwMap.get(ModernRoboticsI2cRangeSensor.class, "senzorDistanta");
  123. gyro = hwMap.get(ModernRoboticsI2cGyro.class, "gyro");
  124. // gyro = (IntegratingGyroscope)modernRoboticsI2cGyro;
  125.  
  126. //marker = hwMap.get(Servo.class, "marker");
  127.  
  128.  
  129. motorLeft1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  130. motorLeft2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  131. motorRight1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  132. motorRight2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  133.  
  134. carlig.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  135.  
  136. motorLeft1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  137. motorLeft2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  138. motorRight1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  139. motorRight2.setMode(DcMotor.RunMode.RUN_USING_ENCODER );
  140.  
  141.  
  142. carlig.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  143.  
  144.  
  145.  
  146. motorLeft1.setDirection(DcMotor.Direction.REVERSE);
  147. motorLeft2.setDirection(DcMotor.Direction.REVERSE);
  148. motorRight1.setDirection(DcMotor.Direction.FORWARD);
  149. motorRight2.setDirection(DcMotor.Direction.FORWARD);
  150.  
  151. sliderSucker.setDirection(DcMotor.Direction.FORWARD);
  152. mineralRiser.setDirection(DcMotor.Direction.REVERSE);
  153. carlig.setDirection(DcMotor.Direction.FORWARD);
  154. suckerFlip.setDirection(DcMotor.Direction.FORWARD);
  155.  
  156. motorLeft1.setPower(0f);
  157. motorLeft2.setPower(0f);
  158. motorRight1.setPower(0f);
  159. motorRight2.setPower(0f);
  160.  
  161. sliderSucker.setPower(0f);
  162. mineralRiser.setPower(0f);
  163. carlig.setPower(0f);
  164. suckerFlip.setPower(0f);
  165.  
  166. MineralFlip.setPosition(0.65f);
  167.  
  168. Sucker.setPosition(0.5f);
  169.  
  170. //marker.setPosition(0f);
  171.  
  172.  
  173. }
  174.  
  175.  
  176. public void holonomic(double left_stick_x, double right_stick_x, double left_stick_y) {
  177.  
  178. //
  179. final double x = Math.pow(left_stick_x, 3.0);
  180. final double y = Math.pow(left_stick_y, 3.0);
  181.  
  182. final double rotation = Math.pow(right_stick_x, 3.0);
  183.  
  184. final double direction = Math.atan2(x, y) ;
  185. final double speed = Math.min(1.0, Math.sqrt(x * x + y * y));
  186.  
  187. final double lf = speed * Math.sin(direction + Math.PI / 4.0) + rotation;
  188. final double rf = speed * Math.cos(direction + Math.PI / 4.0) - rotation;
  189. final double lr = speed * Math.cos(direction + Math.PI / 4.0) + rotation;
  190. final double rr = speed * Math.sin(direction + Math.PI / 4.0) - rotation;
  191.  
  192. motorLeft1.setPower(lf);
  193. motorRight1.setPower(rf);
  194. motorLeft2.setPower(lr);
  195. motorRight2.setPower(rr);
  196. }
  197.  
  198.  
  199. public void setMotorSpeed(float speed) {
  200. motorLeft2.setPower(speed);
  201. motorRight2.setPower(speed);
  202. motorLeft1.setPower(speed);
  203. motorRight1.setPower(speed);
  204. }
  205.  
  206.  
  207. public void landing() {
  208. int newTargetPos = -32650;
  209. carlig.setPower(-1f);
  210. plasareMinerale(30);
  211.  
  212. while (carlig.getCurrentPosition() > newTargetPos) {
  213.  
  214. }
  215.  
  216. carlig.setPower(0f);
  217. }
  218.  
  219. public void plasareMinerale(int mmToExtend) {
  220. int newTargetPos = mineralRiser.getCurrentPosition() + (mmToExtend * extendMotorCountsPerRev60 / mmPerRotationMineralRiser);// 216 mm o rotatie completa
  221. mineralRiser.setTargetPosition(newTargetPos);
  222. mineralRiser.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  223. mineralRiser.setPower(1f * Math.signum(mmToExtend));
  224. while (mineralRiser.isBusy()) {
  225.  
  226. }
  227.  
  228. mineralRiser.setPower(0f);
  229. mineralRiser.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  230. }
  231.  
  232. public void ExtendSucker(int mmToExtend) {
  233. int newTargetPos = sliderSucker.getCurrentPosition() + (mmToExtend * extendMotorCountsPerRev60 / mmPerSuckerExtention);
  234. sliderSucker.setTargetPosition(newTargetPos);
  235. sliderSucker.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  236. if(mmToExtend > 0)
  237. sliderSucker.setPower(1f * Math.signum(mmToExtend));
  238. else sliderSucker.setPower(-0.5f);
  239.  
  240. while (sliderSucker.isBusy()) {
  241.  
  242. }
  243.  
  244. sliderSucker.setPower(0f);
  245. sliderSucker.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  246.  
  247. }
  248.  
  249. public void latchingTeleOp(float sign) {
  250. carlig.setPower(1f * sign);
  251. }
  252.  
  253.  
  254. public void plasareMineraleTeleOp(int sign) {
  255. if (sign > 0)
  256. mineralRiser.setPower(1f);
  257. else
  258. mineralRiser.setPower(-0.5f);
  259.  
  260. }
  261.  
  262. public void plasareMarker() {
  263.  
  264. }
  265. }
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!