API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 24th, 2014
180
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 15.71 KB | None | 0 0
raw download clone embed print report
  1. #pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
  2. #pragma config(Sensor, S1, , sensorI2CMuxController)
  3. #pragma config(Sensor, S2, HTIRS2, sensorHiTechnicIRSeeker1200)
  4. #pragma config(Sensor, S3, nxtSonar, sensorSONAR)
  5. #pragma config(Sensor, S4, nxtColor, sensorCOLORFULL)
  6. #pragma config(Motor, motorA, tubeArm, tmotorNXT, PIDControl, encoder)
  7. #pragma config(Motor, mtr_S1_C1_1, leftMotor, tmotorTetrix, openLoop, encoder)
  8. #pragma config(Motor, mtr_S1_C1_2, rightMotor, tmotorTetrix, openLoop, reversed, encoder)
  9. #pragma config(Motor, mtr_S1_C2_1, tennisArm, tmotorTetrix, openLoop, encoder)
  10. #pragma config(Motor, mtr_S1_C2_2, motorG, tmotorTetrix, openLoop, encoder)
  11. #pragma config(Servo, srvo_S1_C3_1, servo1, tServoNone)
  12. #pragma config(Servo, srvo_S1_C3_2, servo2, tServoNone)
  13. #pragma config(Servo, srvo_S1_C3_3, servo3, tServoNone)
  14. #pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
  15. #pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
  16. #pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
  17.  
  18. #define btnpressed() (nNxtButtonPressed != kNoButton) //Checking if any button is pressed or none
  19. #define btncheck(btn) (nNxtButtonPressed == btn) //Check if a certain button is pressed
  20. #include "hitechnic-irseeker-v2.h"
  21.  
  22. int getbtn(void) { //Wait for a button press, read id, wait for up then return id
  23. while(!btnpressed()) {/* Sleep(1); */} //wait for button press
  24. int btn=nNxtButtonPressed; //read which id
  25. while(btnpressed() ) {/* Sleep(1); */} // wait for button up
  26. return btn; //return button id5
  27. }
  28.  
  29. int chooser(void) {
  30. int runno = 1, runmax = 10;
  31.  
  32. nxtDisplayCenteredBigTextLine(1, "Chooser");
  33.  
  34. while(true)
  35. {
  36. nxtDisplayCenteredBigTextLine(4, "Run %d", runno);
  37. if(getbtn() == 1)//1 = Right Arrow
  38. {
  39. runno++;
  40. if(runno > runmax) runno = 1;
  41. }
  42. else if(getbtn() == 2) //2 = Left Arrow
  43. {
  44. runno--;
  45. if(runno < 1) runno = runmax;
  46. }
  47. else if(getbtn() == 3) //3 = Orange Button
  48. {
  49. nxtDisplayCenteredBigTextLine(6, "Using %d", runno);
  50. return runno;
  51. }
  52. }
  53. }
  54.  
  55. /////////////////////////////////////////////////////////////////////////////////////////////////////
  56. //
  57. // initializeRobot
  58. //
  59. // Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
  60. // Things that might be performed during initialization include:
  61. // 1. Move motors and servos to a preset position.
  62. // 2. Some sensor types take a short while to reach stable values during which time it is best that
  63. // robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
  64. // "bias" value.
  65. //
  66. // In many cases, you may not have to add any code to this function and it will remain "empty".
  67. //
  68. /////////////////////////////////////////////////////////////////////////////////////////////////////
  69.  
  70. void initializeRobot()
  71. {
  72. nMotorEncoder[leftMotor] = 0;
  73. nMotorEncoder[rightMotor] = 0;
  74. nMotorEncoder[tennisArm] = 0;
  75. return;
  76. }
  77.  
  78. void resetMotor()
  79. {
  80. nMotorEncoder[leftMotor] = 0;
  81. nMotorEncoder[rightMotor] = 0;
  82.  
  83. }
  84.  
  85. void wait4Inches(int inches) {
  86. int LeftMotorTarget, RightMotorTarget;
  87. int EncoderValue;
  88. bool LeftMotorDone, RightMotorDone;
  89.  
  90. // 2 * pi * r = 2*3.14*2 = 12.57in per 360 deg
  91. // 360/12.57 = 28.65 deg per in
  92. // 1440 ticks per encoder revolution, 4 ticks per degree
  93. EncoderValue = inches * 28.65 * 4; //Now in ticks
  94. nMotorEncoderTarget[leftMotor] = LeftMotorTarget = nMotorEncoder[leftMotor] + EncoderValue;
  95. nMotorEncoderTarget[rightMotor] = RightMotorTarget = nMotorEncoder[rightMotor] + EncoderValue;
  96.  
  97. // doesn't account for overflow of Encoder value
  98. // wait for 1 motor to reach Target and then wait for the other
  99. // can also return after first motor reaches Target
  100. //while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle)
  101. //// while Motor B AND Motor C are still running (haven't yet reached their target):
  102. //{
  103. // // do not continue
  104. //}
  105. //motor[motorB] = 0; // motor B is stopped at a power level of 0
  106. //motor[motorC] = 0; // motor C is stopped at a power level of 0
  107.  
  108. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  109. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  110. while(!LeftMotorDone && !RightMotorDone) {
  111. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  112. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  113. }
  114. // can also return after first motor reaches Target
  115.  
  116. if (LeftMotorDone) { // Left done, wait for Right
  117. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  118. while (!RightMotorDone) {
  119. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  120. }
  121. } else { // Right done, wait for Left
  122. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  123. while (!LeftMotorDone) {
  124. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  125. }
  126. }
  127. }
  128.  
  129. void wait4BackInches(int inches) {
  130. int LeftMotorTarget, RightMotorTarget;
  131. int EncoderValue;
  132. bool LeftMotorDone, RightMotorDone;
  133.  
  134. // 2 * pi * r = 2*3.14*2 = 12.57in per 360 deg
  135. // 360/12.57 = 28.65 deg per in
  136. // 1440 ticks per encoder revolution, 4 ticks per degree
  137. EncoderValue = inches * 28.65 * 4; //Now in ticks
  138. nMotorEncoderTarget[leftMotor] = LeftMotorTarget = nMotorEncoder[leftMotor] - EncoderValue;
  139. nMotorEncoderTarget[rightMotor] = RightMotorTarget = nMotorEncoder[rightMotor] - EncoderValue;
  140. // doesn't account for overflow of Encoder value
  141.  
  142. // wait for 1 motor to reach Target and then wait for the other
  143. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  144. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  145. while(!LeftMotorDone && !RightMotorDone) {
  146. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  147. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  148. }
  149. // can also return after first motor reaches Target
  150.  
  151. if (LeftMotorDone) { // Left done, wait for Right
  152. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  153. while (!RightMotorDone) {
  154. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  155. }
  156. } else { // Right done, wait for Left
  157. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  158. while (!LeftMotorDone) {
  159. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  160. }
  161. }
  162. }
  163.  
  164. void Forward(int inches, int power)
  165. {
  166. float scale=1.0; // scale master power level for slave
  167.  
  168. resetMotor();
  169. // ramp up
  170. motor[leftMotor] = (int) power/2 * scale;
  171. motor[rightMotor] = power/2;
  172. motor[leftMotor] = (int) power * scale;
  173. motor[rightMotor] = power;
  174. wait4Inches(inches);
  175. // ramp down
  176. motor[leftMotor] = (int) power/2 * scale;
  177. motor[rightMotor] = power/2;
  178. motor[leftMotor] = 0;
  179. motor[rightMotor] = 0;
  180.  
  181. }
  182.  
  183. void Backward(int inches, int power)
  184. {
  185. float scale=1.0; // scale master power level for slave
  186.  
  187. resetMotor();
  188. // ramp up
  189.  
  190. motor[leftMotor] = -1 * (int) power/2 * scale;
  191. motor[rightMotor] = -1 * power/2;
  192. motor[leftMotor] = -1 * (int) power * scale;
  193. motor[rightMotor] = -1 * power;
  194.  
  195. wait4BackInches(inches);
  196. // ramp down
  197.  
  198. motor[leftMotor] = -1 * (int) power/2 * scale;
  199. motor[rightMotor] = -1 * power/2;
  200. motor[leftMotor] = 0;
  201. motor[rightMotor] = 0;
  202.  
  203. }
  204.  
  205. void turnLeft(int angle, int power)
  206. {
  207. int LeftMotorTarget, RightMotorTarget;
  208. int EncoderValue;
  209. bool LeftMotorDone, RightMotorDone;
  210.  
  211. resetMotor();
  212. // 2*pi*r=C, robot wheel to wheel basis 15.5", small wheel 2"
  213. // ratio = 15.5/2 = 7.75 turns of wheel to equal 1 turn of robot.
  214. // 4 ticks per degree
  215. EncoderValue = 4 * angle * 7.75; // New Tick Value Difference
  216.  
  217. if(angle > 0) {
  218. motor[rightMotor] = power;
  219. nMotorEncoderTarget[rightMotor] = RightMotorTarget = nMotorEncoder[rightMotor] + EncoderValue;
  220. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  221. while (!RightMotorDone) {
  222. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  223. }
  224. motor[rightMotor] = 0;
  225. } else {
  226. motor[leftMotor] = - power;
  227. nMotorEncoderTarget[leftMotor] = LeftMotorTarget = nMotorEncoder[leftMotor] + EncoderValue;
  228. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  229. while (!LeftMotorDone) {
  230. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  231. }
  232. motor[leftMotor] = 0;
  233. }
  234.  
  235. }
  236.  
  237. void turnRight(int angle, int power)
  238. {
  239. int LeftMotorTarget, RightMotorTarget;
  240. int EncoderValue;
  241. bool LeftMotorDone, RightMotorDone;
  242. resetMotor();
  243.  
  244. // 2*pi*r=C, robot wheel to wheel basis 15.5", small wheel 2"
  245. // ratio = 15.5/2 = 7.75 turns of wheel to equal 1 turn of robot.
  246. // 4 ticks per degree
  247. EncoderValue = 4 * angle * 7.75; // New Tick Value Difference
  248.  
  249. if(angle > 0) {
  250. motor[leftMotor] = power;
  251. nMotorEncoderTarget[leftMotor] = LeftMotorTarget = nMotorEncoder[leftMotor] + EncoderValue;
  252. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  253. while (!LeftMotorDone) {
  254. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  255. }
  256. motor[leftMotor] = 0;
  257. } else {
  258. motor[rightMotor] = - power;
  259. nMotorEncoderTarget[rightMotor] = RightMotorTarget = nMotorEncoder[rightMotor] + EncoderValue;
  260. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  261. while (!RightMotorDone) {
  262. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  263. }
  264. motor[rightMotor] = 0;
  265. }
  266.  
  267. }
  268.  
  269. void spinLeft(int angle, int power)
  270. {
  271. int LeftMotorTarget, RightMotorTarget;
  272. int EncoderValue;
  273. bool LeftMotorDone, RightMotorDone;
  274.  
  275. // only allow positive numbers
  276. if (angle <= 0 || power <= 0) return;
  277.  
  278. resetMotor();
  279. // 2*pi*r=C, robot wheel to wheel basis 15.5", small wheel 2"
  280. // both wheels spin in opposite direction so radius is half or 7.75
  281. // ratio = 7.75/2 = 3.88 turns of wheel to equal 1 spin of robot.
  282. // 4 ticks per degree
  283. EncoderValue = 4 * angle * 3.88; // New Tick Value Difference
  284.  
  285. nMotorEncoderTarget[leftMotor] = LeftMotorTarget = nMotorEncoder[leftMotor] - EncoderValue;
  286. nMotorEncoderTarget[rightMotor] = RightMotorTarget = nMotorEncoder[rightMotor] + EncoderValue;
  287. motor[leftMotor] = - power;
  288. motor[rightMotor] = power;
  289.  
  290. // wait for 1 motor to reach Target and then return
  291. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  292. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  293. while(!LeftMotorDone && !RightMotorDone) {
  294. LeftMotorDone = nMotorEncoder[leftMotor] < LeftMotorTarget;
  295. RightMotorDone = nMotorEncoder[rightMotor] > RightMotorTarget;
  296. }
  297.  
  298. motor[leftMotor] = 0;
  299. motor[rightMotor] = 0;
  300. }
  301.  
  302. void spinRight(int angle, int power)
  303. {
  304. int LeftMotorTarget, RightMotorTarget;
  305. int EncoderValue;
  306. bool LeftMotorDone, RightMotorDone;
  307.  
  308. // only allow positive numbers
  309. if (angle <= 0 || power <= 0) return;
  310.  
  311. resetMotor();
  312. // 2*pi*r=C, robot wheel to wheel basis 15.5", small wheel 2"
  313. // both wheels spin in opposite direction so radius is half or 7.75
  314. // ratio = 7.75/2 = 3.88 turns of wheel to equal 1 spin of robot.
  315. // 4 ticks per degree
  316. EncoderValue = 4 * angle * 3.88; // New Tick Value Difference
  317.  
  318. nMotorEncoderTarget[leftMotor] = LeftMotorTarget = nMotorEncoder[leftMotor] + EncoderValue;
  319. nMotorEncoderTarget[rightMotor] = RightMotorTarget = nMotorEncoder[rightMotor] - EncoderValue;
  320. motor[leftMotor] = power;
  321. motor[rightMotor] = - power;
  322.  
  323. // wait for 1 motor to reach Target and then return
  324. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  325. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  326. while(!LeftMotorDone && !RightMotorDone) {
  327. LeftMotorDone = nMotorEncoder[leftMotor] > LeftMotorTarget;
  328. RightMotorDone = nMotorEncoder[rightMotor] < RightMotorTarget;
  329. }
  330.  
  331. motor[leftMotor] = 0;
  332. motor[rightMotor] = 0;
  333. }
  334.  
  335. //---------------------------------------------------------------------------------------------------------------------------------
  336.  
  337. task main()
  338. {
  339.  
  340. int runseq;
  341. //runseq = chooser();
  342. initializeRobot();
  343. /*
  344. motor[leftMotor] = 60;
  345. motor[rightMotor] = 20;
  346. wait4Inches(12);
  347. motor[leftMotor] = 0;
  348. motor[rightMotor] = 0;
  349. */
  350.  
  351.  
  352. nxtDisplayCenteredBigTextLine(3, "Test");
  353. return;
  354.  
  355. //waitForStart();
  356. //Autonomous Code
  357. if(runseq == 1) { //Program 1: Knocking the Pole Down & Starting at edge of ramp Pos. 1
  358. int _dirDC = 0;
  359. int _dirAC = 0;
  360. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  361. int acS1, acS2, acS3, acS4, acS5 = 0;
  362. int _dirEnh, _strEnh;
  363. int maxSig = 0;
  364. int x = 0;
  365.  
  366. eraseDisplay();
  367. nxtDisplayCenteredBigTextLine(3, "Running");
  368. wait1Msec(1000);
  369. eraseDisplay();
  370.  
  371. _dirDC = HTIRS2readDCDir(HTIRS2);
  372. if (_dirDC < 0) x = 1;
  373. _dirAC = HTIRS2readACDir(HTIRS2);
  374. if (_dirAC < 0) x = 2;
  375. if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5)) x = 3;
  376. if (!HTIRS2readEnhanced(HTIRS2, _dirEnh, _strEnh)) x = 4;
  377. if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) {
  378. x = 5;
  379. } else {
  380. x = 0;
  381. }
  382.  
  383. maxSig = (acS1 > acS2) ? acS1 : acS2;
  384. maxSig = (maxSig > acS3) ? maxSig : acS3;
  385. maxSig = (maxSig > acS4) ? maxSig : acS4;
  386. maxSig = (maxSig > acS5) ? maxSig : acS5;
  387. nxtDisplayCenteredBigTextLine(1, "Dir=%d", _dirAC);
  388. nxtDisplayCenteredBigTextLine(4, "Sig=%d", maxSig);
  389.  
  390.  
  391. }
  392.  
  393. /*
  394. if(runseq == 2) { //Program 2: Knocking the Pole Down & Starting on edge of ramp Pos. 2
  395. int _dirDC = 0;
  396. int _dirAC = 0;
  397. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  398. int acS1, acS2, acS3, acS4, acS5 = 0;
  399. int _dirEnh, _strEnh;
  400.  
  401.  
  402. }
  403.  
  404. if(runseq == 3) { //Program 3: Knocking the Pole Down & Starting at edge of ramp Pos. 3
  405. int _dirDC = 0;
  406. int _dirAC = 0;
  407. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  408. int acS1, acS2, acS3, acS4, acS5 = 0;
  409. int _dirEnh, _strEnh;
  410.  
  411.  
  412. }
  413.  
  414. //---------------------------------------------------------------------------------------------------------------------------------
  415.  
  416. if(runseq == 4) { //Program 4: Knocking the Pole Down & Starting in base Pos. 1
  417. int _dirDC = 0;
  418. int _dirAC = 0;
  419. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  420. int acS1, acS2, acS3, acS4, acS5 = 0;
  421. int _dirEnh, _strEnh;
  422.  
  423. }
  424.  
  425. if(runseq == 5) { //Program 5: Knocking the Pole Down & Starting in base Pos. 2
  426. int _dirDC = 0;
  427. int _dirAC = 0;
  428. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  429. int acS1, acS2, acS3, acS4, acS5 = 0;
  430. int _dirEnh, _strEnh;
  431.  
  432.  
  433. }
  434.  
  435. if(runseq == 6) { //Program 6: Knocking the Pole Down & Starting in base Pos. 3
  436. int _dirDC = 0;
  437. int _dirAC = 0;
  438. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  439. int acS1, acS2, acS3, acS4, acS5 = 0;
  440. int _dirEnh, _strEnh;
  441.  
  442.  
  443. }
  444.  
  445. //---------------------------------------------------------------------------------------------------------------------------------
  446.  
  447. if(runseq == 7) { //Program 7: Going to Tubes & Starting at base Pos. 1
  448. int _dirDC = 0;
  449. int _dirAC = 0;
  450. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  451. int acS1, acS2, acS3, acS4, acS5 = 0;
  452. int _dirEnh, _strEnh;
  453.  
  454.  
  455. }
  456.  
  457. if(runseq == 8) { //Program 8: Going to Tubes & Starting at base Pos. 2
  458. int _dirDC = 0;
  459. int _dirAC = 0;
  460. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  461. int acS1, acS2, acS3, acS4, acS5 = 0;
  462. int _dirEnh, _strEnh;
  463.  
  464.  
  465. }
  466.  
  467. if(runseq == 9) { //Program 9: Going to Tubes & Starting at base Pos. 3
  468. int _dirDC = 0;
  469. int _dirAC = 0;
  470. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  471. int acS1, acS2, acS3, acS4, acS5 = 0;
  472. int _dirEnh, _strEnh;
  473.  
  474.  
  475. }
  476.  
  477. //---------------------------------------------------------------------------------------------------------------------------------
  478.  
  479. */
  480. if(runseq == 10) { //Program 10: Going towards Tubes & Starting at edge of ramp any Pos?
  481.  
  482.  
  483.  
  484.  
  485.  
  486. }
  487.  
  488. }
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!