Robotc Bridge Scoring Auton mous

SUBMITTED BY: Guest

DATE: Jan. 26, 2014, 7:44 p.m.

FORMAT: Text only

SIZE: 4.0 kB

Raw Download

HITS: 1165

Report
  1. #pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
  2. #pragma config(Hubs, S2, HTMotor, HTMotor, none, none)
  3. #pragma config(Sensor, S1, , sensorI2CMuxController)
  4. #pragma config(Sensor, S2, , sensorI2CMuxController)
  5. #pragma config(Motor, motorA, , tmotorNXT, openLoop)
  6. #pragma config(Motor, motorB, , tmotorNXT, openLoop)
  7. #pragma config(Motor, mtr_S1_C1_1, lin1, tmotorTetrix, openLoop)
  8. #pragma config(Motor, mtr_S1_C1_2, lin2, tmotorTetrix, openLoop)
  9. #pragma config(Motor, mtr_S1_C2_1, motorD, tmotorTetrix, PIDControl, encoder)
  10. #pragma config(Motor, mtr_S1_C2_2, motorF, tmotorTetrix, PIDControl, encoder)
  11. #pragma config(Motor, mtr_S2_C1_1, motorE, tmotorTetrix, PIDControl, encoder)
  12. #pragma config(Motor, mtr_S2_C1_2, motorG, tmotorTetrix, PIDControl, encoder)
  13. #pragma config(Motor, mtr_S2_C2_1, lin3, tmotorTetrix, openLoop)
  14. #pragma config(Motor, mtr_S2_C2_2, lin4, tmotorTetrix, openLoop)
  15. #pragma config(Servo, srvo_S1_C3_1, servo1, tServoNone)
  16. #pragma config(Servo, srvo_S1_C3_2, servo2, tServoNone)
  17. #pragma config(Servo, srvo_S1_C3_3, servo3, tServoNone)
  18. #pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
  19. #pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
  20. #pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
  21. //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
  23. #include "JoystickDriver.c";
  24. void initializeRobot(){
  25. nMotorEncoder(motorD) =0;
  26. nMotorEncoder(motorE) =0;
  27. nMotorEncoder(motorF) =0;
  28. nMotorEncoder(motorG) =0;
  29. return;
  30. }
  31. void move(int direction, float revs);
  32. void turn(int direction, float revs);
  33. int fullrev = 1440;
  34. task main()
  35. {
  36. initializeRobot(); //Prepares robot, communicates with Field control system.
  38. waitForStart(); // Wait for the beginning of autonomous phase.
  39. move(1,6);
  40. }
  41. //This function is for turning. The "int" is actually a boolean. Use 0 for a left turn, 1 for a right turn.
  42. void turn(int direction, float revs)
  43. {
  44. nMotorEncoder(motorD) =0;
  45. nMotorEncoder(motorE) =0;
  46. nMotorEncoder(motorF) =0;
  47. nMotorEncoder(motorG) =0;
  48. if (direction==0)
  49. {
  50. while(nMotorEncoder(motorD) > -(fullrev*revs) & nMotorEncoder(motorE) > -(fullrev*revs) & nMotorEncoder(motorF) > -(fullrev*revs) & nMotorEncoder(motorG) > -(fullrev*revs))
  51. {
  52. motor(motorD) = -60;
  53. motor(motorE) = -60;
  54. motor(motorF) = -60;
  55. motor(motorG) = -60;
  56. }
  57. }
  58. else
  59. {
  60. while(nMotorEncoder(motorD) < (fullrev*revs) & nMotorEncoder(motorE) < (fullrev*revs) & nMotorEncoder(motorF) < (fullrev*revs) & nMotorEncoder(motorG) < (fullrev*revs))
  61. {
  62. motor(motorD) = 60;
  63. motor(motorE) = 60;
  64. motor(motorF) = 60;
  65. motor(motorG) = 60;
  66. }
  67. }
  68. }
  70. //This function is for moving. The "int" is actually a boolean. Use 0 for forward, 1 for backward.
  71. void move(int direction, float revs)
  72. {
  73. nMotorEncoder(motorD) =0;
  74. nMotorEncoder(motorE) =0;
  75. nMotorEncoder(motorF) =0;
  76. nMotorEncoder(motorG) =0;
  77. if (direction==0)
  78. {
  79. while(nMotorEncoder(motorD) > -(fullrev*revs) & nMotorEncoder(motorE) < (fullrev*revs) & nMotorEncoder(motorF) > -(fullrev*revs) & nMotorEncoder(motorG) < (fullrev*revs))
  80. {
  81. motor(motorD) = -60;
  82. motor(motorE) = 60;
  83. motor(motorF) = -60;
  84. motor(motorG) = 60;
  85. }
  86. }
  87. else
  88. {
  89. while(nMotorEncoder(motorD) < (fullrev*revs) & nMotorEncoder(motorE) > -(fullrev*revs) & nMotorEncoder(motorF) < (fullrev*revs) & nMotorEncoder(motorG) > -(fullrev*revs))
  90. {
  91. motor(motorD) = 60;
  92. motor(motorE) = -60;
  93. motor(motorF) = 60;
  94. motor(motorG) = -60;
  95. }
  96. }
  97. }
comments powered by Disqus