API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Feb 20th, 2018
63
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 14.96 KB | None | 0 0
raw download clone embed print report
  1. /*----------------------------------------------------------------------------*/
  2. /* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
  3. /* Open Source Software - may be modified and shared by FRC teams. The code */
  4. /* must be accompanied by the FIRST BSD license file in the root directory of */
  5. /* the project. */
  6. /*----------------------------------------------------------------------------*/
  7.  
  8. package frc.team7242.robot;
  9.  
  10. import edu.wpi.first.wpilibj.IterativeRobot;
  11. import edu.wpi.first.wpilibj.*;
  12. import edu.wpi.first.wpilibj.drive.DifferentialDrive;
  13. import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  14. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  15. import edu.wpi.first.wpilibj.Spark;
  16. import frc.team7242.robot.subsystem.ScalableSpeedControllerGroup;
  17. import edu.wpi.first.wpilibj.CameraServer;
  18. import openrio.powerup.MatchData;
  19.  
  20.  
  21.  
  22. public class Robot extends IterativeRobot {
  23. private static final String kDefaultAuto = "Default";
  24. private static final String kCustomAuto = "My Auto";
  25. private String m_autoSelected;
  26. private SendableChooser<String> m_chooser = new SendableChooser<>();
  27. Joystick driverStick = new Joystick(0);
  28.  
  29.  
  30.  
  31. // double autonomousTime = 3;
  32. //double autonomousTime2 = 4;
  33. // double autonomousTime3 = 5.5;
  34. // double autonomousTime4 = 6;
  35.  
  36. double autonomousTime5 = 2.9;
  37. double autonomousTime6 = 4.06;
  38. double autonomousTime7 = 4.55;
  39. double autonomousTime8 = 11.5;
  40. double autonomousTime9 = 12.3;
  41. double autonomousTime10= 14.5;
  42. double autonomousTime11= 15.2;
  43. double autonomousTime12= 15.7;
  44.  
  45. boolean path7;
  46. boolean path8;
  47. boolean path9;
  48. boolean path10;
  49. boolean path11;
  50. boolean path12;
  51.  
  52.  
  53. public Spark leftFront = new Spark(0);
  54. public Spark rightFront = new Spark(2);
  55. public Spark leftBack = new Spark(1);
  56. public Spark rightBack = new Spark(3);
  57.  
  58. double autonomousStartTime;
  59.  
  60. ScalableSpeedControllerGroup right = new ScalableSpeedControllerGroup(0.75, leftFront, leftBack);
  61. ScalableSpeedControllerGroup left = new ScalableSpeedControllerGroup(0.71, rightFront, rightBack);
  62.  
  63.  
  64. DifferentialDrive drive = new DifferentialDrive(left, right);
  65.  
  66.  
  67. public void robotInit() {
  68. m_chooser.addDefault("Default Auto", kDefaultAuto);
  69. m_chooser.addObject("My Auto", kCustomAuto);
  70. SmartDashboard.putData("Auto choices", m_chooser);
  71. CameraServer.getInstance().startAutomaticCapture();
  72. left.setInverted(true);
  73. right.setInverted(true);
  74.  
  75. // leftFront.setSafetyEnabled(false);
  76. // rightFront.setSafetyEnabled(false);
  77. // leftBack.setSafetyEnabled(false);
  78. // rightBack.setSafetyEnabled(false);
  79. }
  80.  
  81. public void autonomousInit() {
  82. m_autoSelected = m_chooser.getSelected();
  83. System.out.println("Auto selected: " + m_autoSelected);
  84. boolean leftAuto = false; //variuable to see if the first switch from our perspective is on the left side, needed initialization so...
  85. boolean rightAuto = false; //variuable to see if the first switch from our perspective is on the right side, needed onitialization so...
  86.  
  87. MatchData.OwnedSide side = MatchData.getOwnedSide(MatchData.GameFeature.SWITCH_NEAR);
  88. if (side == MatchData.OwnedSide.LEFT) {
  89. // OwnedSide LEFT means that the switch NEAREST to our station is on the left
  90. leftAuto = true;
  91. rightAuto = false;
  92. }
  93. else if (side == MatchData.OwnedSide.RIGHT) {
  94. // OwnedSide RIGHT means that the switch NEAREST to our station is on the RIGHT
  95. rightAuto = true;
  96. leftAuto = false;
  97. }
  98. else {
  99. // This literally shouldn't happen
  100. System.out.println("What is going on");
  101. }
  102.  
  103. boolean leftStarting = false;
  104. boolean mediumStarting = false;
  105. boolean rightStarting = false;
  106.  
  107. int station = DriverStation.getInstance().getLocation();
  108. if (station == 1) {
  109. leftStarting = true;
  110. }
  111. else if (station == 2) {
  112. mediumStarting = true;
  113. }
  114. else if (station == 3) {
  115. rightStarting = true;
  116. }
  117.  
  118. /* boolean leftStarting = driverStick.getRawButtonPressed(8) || driverStick.getRawButtonPressed(7) ;
  119. boolean mediumStarting = driverStick.getRawButtonPressed(10)|| driverStick.getRawButtonPressed(9) ;
  120. boolean rightStarting = driverStick.getRawButtonPressed(12)|| driverStick.getRawButtonPressed(11) ; */
  121.  
  122. path7 = leftStarting && leftAuto;
  123. path8 = leftStarting && rightAuto;
  124. path9 = mediumStarting && leftAuto;
  125. path10 = mediumStarting && rightAuto;
  126. path11 = rightStarting && leftAuto;
  127. path12 = rightStarting && rightAuto;
  128.  
  129. autonomousStartTime = Timer.getFPGATimestamp();
  130.  
  131. }
  132.  
  133. public void autonomousPeriodic() {
  134. double deltaTime = Timer.getFPGATimestamp() - autonomousStartTime;
  135. // if (deltaTime < autonomousTime) {
  136. // drive.arcadeDrive(0.80, 0.0);
  137. //} else if (autonomousTime < deltaTime && deltaTime < autonomousTime2) {
  138. //drive.arcadeDrive(0.0, 0.0);
  139. // } else if (autonomousTime2 < deltaTime && deltaTime < autonomousTime3) {
  140. // drive.arcadeDrive(0.0, -0.56);
  141. // } else if (autonomousTime3 < deltaTime && deltaTime < autonomousTime4) {
  142. // drive.arcadeDrive(1, 0.0);
  143. // } else {
  144. // drive.arcadeDrive(0.0, 0.0);
  145. // }
  146.  
  147. // }
  148.  
  149. /*
  150. boolean path7 = true;
  151. boolean path8 = driverStick.getRawButtonPressed(8);
  152. boolean path9 = driverStick.getRawButtonPressed(9);
  153. boolean path10 = driverStick.getRawButtonPressed(10);
  154. boolean path11 = driverStick.getRawButtonPressed(11);
  155. boolean path12 = driverStick.getRawButtonPressed(12);
  156. */
  157. if(path7) { //start of program for path 7
  158. if (deltaTime < autonomousTime5) {
  159. drive.arcadeDrive(0.8, 0.0); //0.56
  160. } else if (autonomousTime5 < deltaTime && deltaTime < autonomousTime6) {
  161. drive.arcadeDrive(0.0, -0.56);
  162. } else if (autonomousTime6 < deltaTime && deltaTime < autonomousTime7) {
  163. drive.arcadeDrive(2, 0.0);
  164. } else if (autonomousTime7 < deltaTime && deltaTime < autonomousTime8) {
  165. drive.arcadeDrive(0.0, 0.0); //-0.65
  166. } else if (autonomousTime8 < deltaTime && deltaTime < autonomousTime9) {
  167. drive.arcadeDrive(0.0, 0.0);
  168. } else if (autonomousTime9 < deltaTime && deltaTime < autonomousTime10) {
  169. drive.arcadeDrive(0.0, 0.0);
  170. } else if (autonomousTime10 < deltaTime && deltaTime < autonomousTime11) {
  171. drive.arcadeDrive(0.0, 0.0);
  172. } else if (autonomousTime11 < deltaTime && deltaTime < autonomousTime12) {
  173. drive.arcadeDrive(0, 0.0);
  174. } else {
  175. drive.arcadeDrive(0.0, 0.0);
  176. }
  177.  
  178. } //closing bracket for path 7
  179.  
  180. if(path8){ //start of program of path 8
  181. if (deltaTime < autonomousTime5) {
  182. // Forward
  183. drive.arcadeDrive(0.6, 0.0); //0.56
  184. } else if (autonomousTime5 < deltaTime && deltaTime < autonomousTime6) {
  185. // Turn right
  186. drive.arcadeDrive(0.0, -0.61);
  187. } else if (autonomousTime6 < deltaTime && deltaTime < autonomousTime7) {
  188. drive.arcadeDrive(0, 0.0);
  189. } else if (autonomousTime7 < deltaTime && deltaTime < autonomousTime8) {
  190. // Forward
  191. drive.arcadeDrive(0.7, 0.0); //-0.65
  192. } else if (autonomousTime8 < deltaTime && deltaTime < autonomousTime9) {
  193. // Turn left
  194. drive.arcadeDrive(0.0, 0.61);
  195. } else if (autonomousTime9 < deltaTime && deltaTime < autonomousTime10) {
  196. // Forward
  197. drive.arcadeDrive(0.72, 0.0);
  198. } else if (autonomousTime10 < deltaTime && deltaTime < autonomousTime11) {
  199. // Turn left
  200. drive.arcadeDrive(0.0, 0.62);
  201. } else if (autonomousTime11 < deltaTime && deltaTime < autonomousTime12) {
  202. // Ram the switch
  203. drive.arcadeDrive(0.7, 0.0);
  204. } else {
  205. drive.arcadeDrive(0.0, 0.0);
  206. }
  207. } //closing of path 8
  208.  
  209. if(path9){ //start of program of path 9
  210.  
  211. if (deltaTime < autonomousTime5) {
  212. drive.arcadeDrive(0.6, 0.0); //0.56
  213. } else if (autonomousTime5 < deltaTime && deltaTime < autonomousTime6) {
  214. drive.arcadeDrive(0.0, 0.56);
  215. } else if (autonomousTime6 < deltaTime && deltaTime < autonomousTime7) {
  216. drive.arcadeDrive(0, 0.0);
  217. } else if (autonomousTime7 < deltaTime && deltaTime < autonomousTime8) {
  218. drive.arcadeDrive(0.36, 0.0); //-0.65
  219. } else if (autonomousTime8 < deltaTime && deltaTime < autonomousTime9) {
  220. drive.arcadeDrive(0.0, -0.7);
  221. } else if (autonomousTime9 < deltaTime && deltaTime < autonomousTime10) {
  222. drive.arcadeDrive(0.79, 0.0);
  223. } else if (autonomousTime10 < deltaTime && deltaTime < autonomousTime11) {
  224. drive.arcadeDrive(0.0, -0.71);
  225. } else if (autonomousTime11 < deltaTime && deltaTime < autonomousTime12) {
  226. drive.arcadeDrive(3, 0.0);
  227. } else {
  228. drive.arcadeDrive(0.0, 0.0);
  229. }
  230.  
  231. } //closing of path 9
  232.  
  233. if(path10){ //start of program of path 10
  234.  
  235. if (deltaTime < autonomousTime5) {
  236. drive.arcadeDrive(0.6, 0.0);
  237. } else if (autonomousTime5 < deltaTime && deltaTime < autonomousTime6) {
  238. drive.arcadeDrive(0.0, -0.62);
  239. } else if (autonomousTime6 < deltaTime && deltaTime < autonomousTime7) {
  240. drive.arcadeDrive(0, 0.0);
  241. } else if (autonomousTime7 < deltaTime && deltaTime < autonomousTime8) {
  242. drive.arcadeDrive(0.36, 0.0);
  243. } else if (autonomousTime8 < deltaTime && deltaTime < autonomousTime9) {
  244. drive.arcadeDrive(0.0, 0.61);
  245. } else if (autonomousTime9 < deltaTime && deltaTime < autonomousTime10) {
  246. drive.arcadeDrive(0.72, 0.0);
  247. } else if (autonomousTime10 < deltaTime && deltaTime < autonomousTime11) {
  248. drive.arcadeDrive(0.0, 0.62);
  249. } else if (autonomousTime11 < deltaTime && deltaTime < autonomousTime12) {
  250. drive.arcadeDrive(2, 0.0);
  251. } else {
  252. drive.arcadeDrive(0.0, 0.0);
  253. }
  254.  
  255. } //closing of path 10
  256.  
  257. if(path11){ //start of program of path 11
  258. if (deltaTime < autonomousTime5) {
  259. drive.arcadeDrive(0.6, 0.0); //0.56
  260. } else if (autonomousTime5 < deltaTime && deltaTime < autonomousTime6) {
  261. drive.arcadeDrive(0.0, 0.56);
  262. } else if (autonomousTime6 < deltaTime && deltaTime < autonomousTime7) {
  263. drive.arcadeDrive(0, 0.0);
  264. } else if (autonomousTime7 < deltaTime && deltaTime < autonomousTime8) {
  265. drive.arcadeDrive(0.7, 0.0); //-0.65
  266. } else if (autonomousTime8 < deltaTime && deltaTime < autonomousTime9) {
  267. drive.arcadeDrive(0.0, -0.7);
  268. } else if (autonomousTime9 < deltaTime && deltaTime < autonomousTime10) {
  269. drive.arcadeDrive(0.79, 0.0);
  270. } else if (autonomousTime10 < deltaTime && deltaTime < autonomousTime11) {
  271. drive.arcadeDrive(0.0, -0.71);
  272. } else if (autonomousTime11 < deltaTime && deltaTime < autonomousTime12) {
  273. drive.arcadeDrive(3, 0.0);
  274. } else {
  275. drive.arcadeDrive(0.0, 0.0);
  276. }
  277. } //closing of path 11
  278.  
  279. if(path12){ //start of program of path 12
  280. if (deltaTime < autonomousTime5) {
  281. drive.arcadeDrive(0.8, 0.0); //0.56
  282. } else if (autonomousTime5 < deltaTime && deltaTime < autonomousTime6) {
  283. drive.arcadeDrive(0.0, 0.56);
  284. } else if (autonomousTime6 < deltaTime && deltaTime < autonomousTime7) {
  285. drive.arcadeDrive(2, 0.0);
  286. } else if (autonomousTime7 < deltaTime && deltaTime < autonomousTime8) {
  287. drive.arcadeDrive(0.0, 0.0); //-0.65
  288. } else if (autonomousTime8 < deltaTime && deltaTime < autonomousTime9) {
  289. drive.arcadeDrive(0.0, 0.0);
  290. } else if (autonomousTime9 < deltaTime && deltaTime < autonomousTime10) {
  291. drive.arcadeDrive(0.0, 0.0);
  292. } else if (autonomousTime10 < deltaTime && deltaTime < autonomousTime11) {
  293. drive.arcadeDrive(0.0, 0.0);
  294. } else if (autonomousTime11 < deltaTime && deltaTime < autonomousTime12) {
  295. drive.arcadeDrive(0, 0.0);
  296. } else {
  297. drive.arcadeDrive(0.0, 0.0);
  298. }
  299. } //closing of path 12
  300.  
  301. } //closing bracket for autonomous periodic
  302.  
  303. public void teleopPeriodic() {
  304.  
  305. double xvalue = (driverStick.getX())/-4;
  306. double yvalue = (driverStick.getY())*-1;
  307. double boost;
  308. double braking;
  309.  
  310. boolean trigger = driverStick.getRawButton(1);
  311. if (trigger) {
  312. boost = 2;
  313. } else {
  314. boost = 1;
  315. }
  316.  
  317. boolean brake = driverStick.getRawButton(2);
  318. if (brake){
  319. braking = 0;
  320. }
  321. else {
  322. braking = 1;
  323. }
  324.  
  325. double sens = 1;
  326. double xsens = 0.5;
  327. double ysens = 0.8;
  328. double threshold = 0;
  329. double tres;
  330. if((Math.abs(xvalue)<threshold) && (Math.abs(yvalue)<threshold)){
  331. tres = 0;
  332. }
  333. else {
  334. tres = 1;
  335. }
  336.  
  337. double xabs = Math.abs(xvalue);
  338. double yabs = Math.abs(yvalue);
  339.  
  340. // x should be a value from -1 to 1, based on the sum of x and y value
  341. // -1 is the robot going full speed backwards, and 1 is the robot going full speed ahead/
  342. //double x = (((yvalue) + xabs) * boost * -1 * braking * tres );
  343. // double x = ((yvalue + xabs) * 1.2 * boost * braking * -1 * tres);
  344.  
  345. // y should be a value from 0 to 1, based on xvalue
  346. // -1 is the robot going full speed to the left and 1 is the robot going full speed to the right
  347. // double r = (xvalue * -1);
  348.  
  349. double rightMotor =((yvalue - xvalue/sens) * (0.8) * boost * braking * tres);
  350. double leftMotor = ((yvalue + xvalue/sens) * (0.8) * boost * braking * tres);
  351. drive.tankDrive(leftMotor, rightMotor);
  352. }
  353. }
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!