API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 28th, 2015
82
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 17.73 KB | None | 0 0
raw download clone embed print report
  1. /*******************************************************************************
  2. * @mainpage
  3. * This is the program for the BCAMSC Robotics Team 2581P 2015 competition
  4. * robot.
  5. *
  6. * @details
  7. * This program is the primary file for the BCAMSC Robotics Team 2581P program
  8. * for the 2015-2016 school year.
  9. *
  10. * This program is based on a template by James Pearman in his "C on Vex"
  11. * library, V 1.00.
  12. *
  13. * @author James Pearman
  14. * @author Annelise Comai <anneliesecomai@gmail.com>
  15. * @author Derek Cheyne <>
  16. * @author Maycee McClure <maymcc01@gmail.com>
  17. * @author Ryan Hopkins <>
  18. *
  19. * @since 2015-05-19
  20. ******************************************************************************/
  21.  
  22.  
  23. #define MOT_INTAKE kVexMotor_10
  24. #define MOT_ELEVATOR kVexMotor_5
  25.  
  26. #define MOT_NORTH_EAST kVexMotor_9
  27. #define MOT_NORTH_WEST kVexMotor_8
  28. #define MOT_SOUTH_EAST kVexMotor_3
  29. #define MOT_SOUTH_WEST kVexMotor_2
  30.  
  31. #define MOT_FLY_ONE kVexMotor_4
  32. #define MOT_FLY_TWO kVexMotor_6
  33. #define MOT_FLY_THREE kVexMotor_1
  34. #define MOT_FLY_FOUR kVexMotor_7
  35.  
  36. #include <stdlib.h>
  37. #include <math.h>
  38. #include <string.h>
  39. #include "ch.h" // needs for all ChibiOS programs
  40. #include "hal.h" // hardware abstraction layer header
  41. #include "vex.h" // vex library header
  42. //#include "apollo.h" // the library that includes the apollo debug window in screen
  43. //#include "smartmotor.h" // the library for smart motors
  44. #include "vexgyro.h"
  45. #include "robotc_glue.h"
  46.  
  47.  
  48.  
  49. //Variables Used Almost Everywhere
  50.  
  51. bool escapeTime(void) //Enables a kill switch for the robot, as long as !escapeTime is a condition in every function
  52. {
  53. if ((vexControllerGet(Btn7U) == 1) && //If all of these buttons are pressed
  54. (vexControllerGet(Btn7D) == 1) &&
  55. (vexControllerGet(Btn8D) == 1) &&
  56. (vexControllerGet(Btn5U) == 1) &&
  57. (vexControllerGet(Btn6U) == 1))
  58. {
  59. return true; //This function will return true
  60. }
  61. else
  62. {
  63. return false; //Else this will return false
  64. }
  65. }
  66.  
  67. //Global variables used for controlling intake, elevator, and launcher during auton
  68. bool isLaunchOn= false;
  69. bool isIntakeOn= false;
  70.  
  71.  
  72.  
  73. #include "DerekAuton.c" //Contains various functions with PID capabilities
  74.  
  75. // Digi IO configuration
  76. static vexDigiCfg dConfig[kVexDigital_Num] = {
  77. { kVexDigital_1, kVexSensorQuadEncoder, kVexConfigQuadEnc1, kVexQuadEncoder_1 },
  78. { kVexDigital_2, kVexSensorQuadEncoder, kVexConfigQuadEnc2, kVexQuadEncoder_1 },
  79. { kVexDigital_3, kVexSensorQuadEncoder, kVexConfigQuadEnc1, kVexQuadEncoder_2 },
  80. { kVexDigital_4, kVexSensorQuadEncoder, kVexConfigQuadEnc2, kVexQuadEncoder_2 },
  81. { kVexDigital_5, kVexSensorQuadEncoder, kVexConfigQuadEnc1, kVexQuadEncoder_3 },
  82. { kVexDigital_6, kVexSensorQuadEncoder, kVexConfigQuadEnc2, kVexQuadEncoder_3 },
  83. { kVexDigital_7, kVexSensorQuadEncoder, kVexConfigQuadEnc1, kVexQuadEncoder_4 },
  84. { kVexDigital_8, kVexSensorQuadEncoder, kVexConfigQuadEnc2, kVexQuadEncoder_4 },
  85. { kVexDigital_9, kVexSensorQuadEncoder, kVexConfigQuadEnc1, kVexQuadEncoder_5 },
  86. { kVexDigital_10, kVexSensorQuadEncoder, kVexConfigQuadEnc2, kVexQuadEncoder_5 },
  87. { kVexDigital_11, kVexSensorDigitalInput, kVexConfigInput, 0 },
  88. { kVexDigital_12, kVexSensorDigitalInput, kVexConfigInput, 0 }
  89. };
  90.  
  91. //Motor configuration
  92. static vexMotorCfg mConfig[kVexMotorNum] = {
  93. { MOT_NORTH_EAST, kVexMotor393T, kVexMotorNormal, kVexSensorIME, kImeChannel_1 },
  94. { MOT_NORTH_WEST, kVexMotor393T, kVexMotorNormal, kVexSensorIME, kImeChannel_2 },
  95. { MOT_SOUTH_EAST, kVexMotor393T, kVexMotorNormal, kVexSensorNone, 0 },
  96. { MOT_SOUTH_WEST, kVexMotor393T, kVexMotorNormal, kVexSensorNone, 0 },
  97.  
  98. { MOT_FLY_ONE, kVexMotor393T, kVexMotorNormal, kVexSensorIME, kImeChannel_6 },
  99. { MOT_FLY_TWO, kVexMotor393T, kVexMotorNormal, kVexSensorNone, 0 },
  100. { MOT_FLY_THREE, kVexMotor393T, kVexMotorNormal, kVexSensorIME, kImeChannel_4 },
  101. { MOT_FLY_FOUR, kVexMotor393T, kVexMotorReversed, kVexSensorIME, kImeChannel_5 },
  102.  
  103. { MOT_INTAKE, kVexMotor393T, kVexMotorNormal, kVexSensorNone, 0 },
  104. { MOT_ELEVATOR, kVexMotor393T, kVexMotorNormal, kVexSensorIME, kImeChannel_3}
  105. };
  106.  
  107.  
  108. //#include "automaticaiming.c"
  109. //#include "autonfunctions.c"
  110.  
  111. void moveFunc(int ch1, int ch2, int ch4)
  112. {
  113. vexMotorSet(MOT_NORTH_EAST, -ch1 + ch2 + ch4);
  114. vexMotorSet(MOT_NORTH_WEST, ch1 + ch2 + ch4);
  115. vexMotorSet(MOT_SOUTH_EAST, -ch1 - ch2 + ch4);
  116. vexMotorSet(MOT_SOUTH_WEST, ch1 - ch2 + ch4);
  117. }
  118.  
  119. void setIntake(int butnup, int butndown)
  120. {
  121. if( !(escapeTime()))
  122. {
  123. vexMotorSet(MOT_INTAKE, ((butnup - butndown) * 127) );
  124. vexMotorSet(MOT_ELEVATOR, ( (butnup - butndown) * 127) );
  125. }
  126. }
  127.  
  128. void setMotors(int sped)
  129. {
  130. if(!escapeTime())
  131. {
  132. vexMotorSet(MOT_FLY_ONE, sped);
  133. vexMotorSet(MOT_FLY_TWO, sped);
  134. vexMotorSet(MOT_FLY_THREE, sped);
  135. vexMotorSet(MOT_FLY_FOUR, sped);
  136. }
  137. }
  138.  
  139.  
  140. void slowDown(void)
  141. {
  142. int speed = vexMotorGet(MOT_FLY_ONE);
  143. if(speed > 0)
  144. {
  145. speed -= 3;
  146. chThdSleepMilliseconds(100);
  147. setMotors(speed);
  148.  
  149. }
  150. }
  151. /*
  152.  
  153. void autonShootBalls(int time)
  154. {
  155. vexMotorSet(MOT_FLY_ONE, 127);
  156. vexMotorSet(MOT_FLY_TWO, 127);
  157. vexMotorSet(MOT_FLY_THREE, 127);
  158. vexMotorSet(MOT_FLY_FOUR, 127);
  159.  
  160. chThdSleepMilliseconds(time);
  161.  
  162. float speed = vexMotorGet(MOT_FLY_ONE);
  163.  
  164. if(speed > 0)
  165. {
  166. speed -= 3;
  167. //chThdSleepMilliseconds(100);//
  168. vexMotorSet(MOT_FLY_ONE, speed);
  169. vexMotorSet(MOT_FLY_TWO, speed);
  170. vexMotorSet(MOT_FLY_THREE, speed);
  171. vexMotorSet(MOT_FLY_FOUR, speed);
  172. }
  173.  
  174. }
  175.  
  176. */
  177.  
  178.  
  179.  
  180.  
  181.  
  182.  
  183. void shootBall(int ballButton)
  184. {
  185. int speed = vexMotorGet(MOT_FLY_ONE);
  186.  
  187. if(ballButton) //If button for launching the ball is pressed
  188. {
  189. vexMotorSet(MOT_FLY_ONE, 127);
  190. vexMotorSet(MOT_FLY_TWO, 127);
  191. vexMotorSet(MOT_FLY_THREE, 127);
  192. vexMotorSet(MOT_FLY_FOUR, 127);
  193. }
  194. else if(speed > 0)
  195. {
  196. speed -= 3;
  197. chThdSleepMilliseconds(100);
  198. vexMotorSet(MOT_FLY_ONE, speed);
  199. vexMotorSet(MOT_FLY_TWO, speed);
  200. vexMotorSet(MOT_FLY_THREE, speed);
  201. vexMotorSet(MOT_FLY_FOUR, speed);
  202.  
  203. }
  204. }
  205. //Start motors
  206. //Set all of the motors to 127
  207.  
  208.  
  209.  
  210. //slowDown();
  211.  
  212. /*//Default State
  213. while(ballButton != 1) {
  214. slowDown();
  215. }
  216. */
  217.  
  218.  
  219.  
  220. task vexLcdThread(void *arg)
  221. {
  222. (void)arg;
  223. vexTaskRegister("lcdout");
  224.  
  225. bool shift = false;
  226. int print = 0;
  227.  
  228. while(1)
  229. {
  230. /*
  231. float xCoord = findXPositionOnField();
  232. float yCoord = findYPositionOnField();
  233. */
  234. // gyro = (vexGyroGet() / 10);
  235.  
  236. //This block sets the screen that is displayed
  237. if (vexLcdButtonGet(1) == kLcdButtonLeft)
  238. {
  239. if(!shift)
  240. {
  241. print += 1;
  242. }
  243. shift = true;
  244. }
  245. else if (vexLcdButtonGet(1) == kLcdButtonRight)
  246. {
  247. if(!shift)
  248. {
  249. print -= 1;
  250. }
  251. shift = true;
  252. }
  253. else
  254. {
  255. shift = false;
  256. }
  257.  
  258. //Each level of print represents a screen that could be displayed.
  259.  
  260. if (print == -1)
  261. {
  262. print = 0;
  263. }
  264.  
  265. else if (print == 0)
  266. {
  267. vexLcdPrintf(1,1, "%s%d","North East", vexMotorPositionGet(MOT_NORTH_EAST) );
  268. vexLcdPrintf(1,0, "%s%d","North West", vexMotorPositionGet(MOT_NORTH_WEST) );
  269. }
  270.  
  271. else if (print == 1)
  272. {
  273. vexLcdPrintf(1,1, "%s%d","Elevator", vexMotorPositionGet(MOT_ELEVATOR) );
  274. vexLcdPrintf(1,0, "%s%1.1d","Fly3", vexMotorPositionGet(MOT_FLY_THREE) );
  275. }
  276.  
  277. else if (print == 2)
  278. {
  279. vexLcdPrintf(1,1, "%s%d","Fly4", vexMotorPositionGet(MOT_FLY_FOUR) );
  280. vexLcdPrintf(1,0, "%s%d","Fly1", vexMotorPositionGet(MOT_FLY_ONE) );
  281. }
  282. else if (print == 3)
  283. {
  284. // vexLcdPrintf(1,1, "%s%d","PT: ",vexAdcGet(0));
  285. vexLcdPrintf(1,1, "%s%d","FLY1SPED", vexMotorGet(MOT_FLY_ONE));
  286. vexLcdPrintf(1,0, "%s%d", "y", 0);
  287. }
  288. else if (print == 4)
  289. {
  290. print = 0;
  291. }
  292. chThdSleepMilliseconds(25);
  293. }
  294.  
  295. return (msg_t)0;
  296. }
  297.  
  298. task runIntake(void *arg)
  299. {
  300. (void)arg;
  301. vexTaskRegister("intake");
  302.  
  303. while(1)
  304. {
  305. setIntake(vexControllerGet(Btn6U), vexControllerGet(Btn6D) );
  306. }
  307.  
  308. return (msg_t)0;
  309. }
  310.  
  311.  
  312.  
  313. task runLauncher(void *arg)
  314. {
  315. (void)arg;
  316. vexTaskRegister("launcher");
  317. while(1)
  318. {
  319. shootBall(vexControllerGet(Btn8U) );
  320. }
  321.  
  322. return (msg_t)0;
  323.  
  324.  
  325. }
  326.  
  327.  
  328.  
  329.  
  330. task runSlowLauncher(void *arg) //used to launch at slower speed so motors can rest
  331. {
  332. (void)arg;
  333. vexTaskRegister("launcher");
  334. while(1)
  335. {
  336.  
  337. int speed = vexMotorGet(MOT_FLY_ONE);
  338.  
  339. if(vexControllerGet(Btn8L)) //If 8L button is pressed
  340. {
  341. vexMotorSet(MOT_FLY_ONE, 100);
  342. vexMotorSet(MOT_FLY_TWO, 100);
  343. vexMotorSet(MOT_FLY_THREE, 100);
  344. vexMotorSet(MOT_FLY_FOUR, 100);
  345. }
  346. else if(speed > 0)
  347. {
  348. speed -= 3;
  349. chThdSleepMilliseconds(100);
  350. vexMotorSet(MOT_FLY_ONE, speed);
  351. vexMotorSet(MOT_FLY_TWO, speed);
  352. vexMotorSet(MOT_FLY_THREE, speed);
  353. vexMotorSet(MOT_FLY_FOUR, speed);
  354.  
  355. }
  356.  
  357. }
  358.  
  359. return (msg_t)0;
  360.  
  361.  
  362. }
  363.  
  364.  
  365.  
  366. task driveTask(void *arg)
  367. {
  368. (void)arg;
  369. vexTaskRegister("drive");
  370. while(1)
  371. {
  372. int ch2, ch1, ch4;
  373. int deadZone = 10;
  374.  
  375.  
  376. if(abs(vexControllerGet(Ch2)) > deadZone)
  377. {
  378. ch2 = vexControllerGet(Ch2);
  379. }
  380. else ch2 = 0;
  381.  
  382. if(abs(vexControllerGet(Ch1)) > 10)
  383. {
  384. ch1 = vexControllerGet(Ch1);
  385. }
  386. else ch1 = 0;
  387.  
  388. if(abs(vexControllerGet(Ch4)) > deadZone)
  389. {
  390. ch4 = vexControllerGet(Ch4);
  391. }
  392. else ch4 = 0;
  393.  
  394. vexMotorSet(MOT_NORTH_EAST, -ch2 + ch1 + ch4);
  395. vexMotorSet(MOT_NORTH_WEST, ch2 + ch1 + ch4);
  396. vexMotorSet(MOT_SOUTH_EAST, -ch2 - ch1 + ch4);
  397. vexMotorSet(MOT_SOUTH_WEST, ch2 - ch1 + ch4);
  398. /*
  399. vexMotorSet(MOT_NORTH_EAST, -vexControllerGet(Ch2) + vexControllerGet(Ch1) + vexControllerGet(Ch4));
  400. vexMotorSet(MOT_NORTH_WEST, vexControllerGet(Ch2) + vexControllerGet(Ch1) + vexControllerGet(Ch4));
  401. vexMotorSet(MOT_SOUTH_EAST, -vexControllerGet(Ch2) - vexControllerGet(Ch1) + vexControllerGet(Ch4));
  402. vexMotorSet(MOT_SOUTH_WEST, vexControllerGet(Ch2) - vexControllerGet(Ch1) + vexControllerGet(Ch4));
  403. */
  404. }
  405. }
  406.  
  407.  
  408.  
  409. task autonRunIntake(void *arg)
  410. {
  411. (void)arg;
  412. vexTaskRegister("autonIntake");
  413. while(1)
  414. {
  415. //while(!escapeTime)
  416. //{
  417. if(isIntakeOn)
  418. {
  419.  
  420. vexMotorSet(MOT_INTAKE, 127);
  421. vexMotorSet(MOT_ELEVATOR, 127);
  422. }
  423. else {
  424.  
  425. vexMotorSet(MOT_INTAKE, 0);
  426. vexMotorSet(MOT_ELEVATOR, 0);
  427.  
  428. }
  429.  
  430. //}
  431. }
  432. }
  433.  
  434. task autonShootBalls(void *arg)
  435. {
  436. (void)arg;
  437. vexTaskRegister("autonLauncher");
  438. while(1)
  439. {
  440. //while(!escapeTime)
  441. //{
  442. float speed = vexMotorGet(MOT_FLY_ONE);
  443. if(isLaunchOn == true) //If the global isLaunchOn variable is declared to be true within the function
  444. {
  445. vexMotorSet(MOT_FLY_ONE, 80); //Set all of the launch motors to 80 (can be changed later if needed)
  446. vexMotorSet(MOT_FLY_TWO, 80);
  447. vexMotorSet(MOT_FLY_THREE, 80);
  448. vexMotorSet(MOT_FLY_FOUR, 80);
  449.  
  450. //chThdSleepMilliseconds(time);//
  451. }
  452.  
  453.  
  454. else if(speed > 0) //Else, slow down the launcher motors
  455. {
  456. speed -= 3;
  457. chThdSleepMilliseconds(100);
  458. vexMotorSet(MOT_FLY_ONE, speed);
  459. vexMotorSet(MOT_FLY_TWO, speed);
  460. vexMotorSet(MOT_FLY_THREE, speed);
  461. vexMotorSet(MOT_FLY_FOUR, speed);
  462. }
  463. //}
  464.  
  465. /* else {
  466.  
  467. vexMotorSet(MOT_FLY_ONE,0);
  468. vexMotorSet(MOT_FLY_TWO,0);
  469. vexMotorSet(MOT_FLY_THREE,0);
  470. vexMotorSet(MOT_FLY_FOUR,0);
  471. }
  472. */
  473. }
  474. }
  475.  
  476. /**
  477. * @brief User setup
  478. * @details
  479. * The digital and motor ports can (should) be configured here.
  480. */
  481. void vexUserSetup()
  482. {
  483. vexDigitalConfigure(dConfig, DIG_CONFIG_SIZE( dConfig));
  484. vexMotorConfigure(mConfig, MOT_CONFIG_SIZE( mConfig));
  485. }
  486.  
  487. /**
  488. * @brief User initialize
  489. * @details
  490. * This function is called after all setup is complete and communication has
  491. * been established with the master processor.
  492. * Start other tasks and initialize user variables here
  493. */
  494. void vexUserInit()
  495. {
  496. vexGyroInit(kVexAnalog_1);
  497. vexMotorPositionSet(MOT_SOUTH_WEST, 0);
  498. vexMotorPositionSet(MOT_SOUTH_EAST, 0);
  499. vexMotorPositionSet(MOT_NORTH_WEST, 0);
  500. vexMotorPositionSet(MOT_NORTH_EAST, 0);
  501. }
  502.  
  503. /**
  504. * @brief Autonomous
  505. * @details
  506. * This thread is started when the autonomous period is started
  507. */
  508. msg_t vexAutonomous(void *arg)
  509. {
  510. (void)arg;
  511.  
  512. // Must call this
  513. vexTaskRegister("auton");
  514. StartTask(vexLcdThread);
  515. StartTask(autonRunIntake);
  516. StartTask(autonShootBalls);
  517.  
  518.  
  519.  
  520.  
  521.  
  522. return (msg_t)0;
  523. }
  524.  
  525.  
  526. /**
  527. * @brief Driver Control
  528. *
  529. * @details
  530. * This thread is started when the driver control period is started
  531. */
  532. msg_t vexOperator(void *arg)
  533. {
  534. (void)arg;
  535.  
  536. // Must call this
  537. vexTaskRegister("operator");
  538.  
  539.  
  540. StartTask(vexLcdThread);
  541. StartTask(runIntake);
  542. //StartTask(runLauncher);
  543. StartTask(driveTask);
  544. //StartTask(runSlowLauncher);
  545. //StartTask(autonShootBalls);
  546. //StartTask(autonRunIntake);
  547.  
  548.  
  549. // Run until asked to terminate
  550. while(!chThdShouldTerminate())
  551. {
  552.  
  553. if (vexControllerGet(Btn7L)) //Just here to test things
  554. {
  555. turnLeftNoPID(90);
  556. }
  557.  
  558.  
  559. if(vexControllerGet(Btn8D))
  560. {
  561. //////////This is code for an auton program focused on launching the four preloads (currently written for three). Place robot on starting tile directly facing goal with three balls in the intake, with the ball furthest in pressed up against the Moibus strip chain guard box (this auton works with no balls actually in elevator, just one at the bottom of the elevator, this can be changed later)
  562.  
  563. //Warm Up Launcher
  564. isLaunchOn = true; //Turns on launcher (make sure that autonShootBalls task is started before trying this)
  565. chThdSleepMilliseconds(2500); //Time needed for launcher to get up to full speed
  566. vexMotorPositionSet(MOT_ELEVATOR, 0); //Resets encoder on elevator motor
  567.  
  568. //Shooting the first ball
  569.  
  570. isIntakeOn = true; //Turns on the intake and the elevator motors (make sure that autonRunIntake task is started before trying this)
  571. while(vexMotorPositionGet(MOT_ELEVATOR) < 800) //The elevator motor will run and the elevator encoder will count up to 800 (this is approx. number of elevator encoder counts needed to get ball at the bottom of the elevator shaft up the elevator and through launcher without the other balls coming up the elevator getting stuck on the bottom wheel of the launcher - this could be changed if we shoved a ball halfway up the elevator shaft as well)
  572. {
  573. chThdSleepMilliseconds(25); //Tells robot to keep doing what it's doing until encoder reaches its designated value
  574. }
  575.  
  576. isIntakeOn = false; //Turns off the intake so that the launcher motors can get back up to speed after launching ball
  577. vexMotorPositionSet(MOT_ELEVATOR, 0); //Resets elevator encoder
  578.  
  579. chThdSleepMilliseconds(3000); //Time needed for launcher motors to get back up to speed
  580.  
  581.  
  582. //Second ball
  583. isIntakeOn = true; //Turns intake back on again
  584. while(vexMotorPositionGet(MOT_ELEVATOR) < 800) //Brings single ball up elevator
  585. {
  586. chThdSleepMilliseconds(25);
  587. }
  588. isIntakeOn = false; //Turns off intake after ball is launched
  589.  
  590. vexMotorPositionSet(MOT_ELEVATOR, 0); //Resets elevator encoder
  591.  
  592. chThdSleepMilliseconds(2500); //Launcher motors get back up to speed
  593.  
  594. //Third Ball
  595. isIntakeOn = true; //Turns on intake
  596. while(vexMotorPositionGet(MOT_ELEVATOR) < 800) //Brings single ball up elevator
  597. {
  598. chThdSleepMilliseconds(25);
  599. }
  600.  
  601. //Stop everything
  602. isIntakeOn = false; //Stops intake
  603. isLaunchOn = false; //Stops launcher
  604.  
  605. }
  606.  
  607.  
  608. if(vexControllerGet(Btn7U)) {
  609.  
  610. //This program does the same thing as above (launches 3 preloads, same position as above), but with wait statements instead of encoder counts, making it a more unreliable and inefficient program.
  611.  
  612.  
  613.  
  614. isLaunchOn = true; //starts launcher
  615.  
  616.  
  617. //I found out that this method below doesn't work - essentially, the vexMotorGet function thinks that when you set a motor to 80, the motor immediately is powered to 80, while the wheels (what we actually want to measure the speed of) have to take time to accelerate to their full speed. We could put an encoder on the wheel axle and write a function to measure exactly how fast that encoder increases per second to find the speed of the wheel later if we have time.
  618. /*while(vexMotorGet(MOT_FLY_ONE) < 80)
  619. { //once fly wheel motors are at full speed
  620. chThdSleepMilliseconds(25)
  621. }
  622. */
  623. chThdSleepMilliseconds(2500);
  624. isIntakeOn = true;
  625.  
  626. chThdSleepMilliseconds(4000);
  627. isIntakeOn = false;
  628. chThdSleepMilliseconds(2500);
  629. isIntakeOn = true;
  630. chThdSleepMilliseconds(4000);
  631. isIntakeOn = false;
  632. isLaunchOn = false;
  633. /*
  634. chThdSleepMilliseconds(10000);
  635. isIntakeOn = false;
  636. isLaunchOn = false;
  637. */
  638.  
  639. }
  640.  
  641.  
  642.  
  643.  
  644. //if (vexControllerGet(Btn7L))
  645. // {
  646. // moveForward(10);
  647. // }
  648.  
  649.  
  650.  
  651.  
  652. //findXPositionOnField();
  653. //findYPositionOnField();
  654. //xCoord = findXPositionOnField();
  655. // yCoord = findYPositionOnField();
  656. /*
  657. moveFunc( vexControllerGet(Ch1), vexControllerGet(Ch2), vexControllerGet(Ch3));
  658. setIntake( vexControllerGet(Btn5U), vexControllerGet(Btn5D) );
  659. shootBall( vexControllerGet(Btn8U) );
  660.  
  661. */
  662. //Don't hog cpu
  663. vexSleep(25);
  664. }
  665.  
  666. return (msg_t)0;
  667. }
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!