API tools faq
paste
Guest User

Untitled

a guest
Jul 19th, 2018
66
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.41 KB | None | 0 0
raw download clone embed print report
  1. #pragma config(Sensor, S1, touchS, sensorTouch)
  2. #pragma config(Sensor, S3, lightS1, sensorLightInactive)
  3. #pragma config(Sensor, S4, lightS2, sensorLightInactive)
  4. #pragma config(Motor, motorA, aMotor, tmotorNormal, PIDControl, )
  5. #pragma config(Motor, motorB, bMotor, tmotorNormal, PIDControl, )
  6. //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
  7.  
  8. const tSensors touchS = (tSensors) S1;
  9. const tSensors lightS1 = (tSensors) S3;
  10. const tSensors lightS2 = (tSensors) S4;
  11.  
  12. static int READ_WAIT = 100;
  13. static int SPREMEMBA_SMERI_MOGOCE = 10; // ce 10*READ_WAIT ni nic
  14. static const int CAPACITY = 1200;
  15. static int RecLength = CAPACITY;
  16. static const int SPEED_A = 60;
  17. static const int SPEED_LINE = 20;
  18. static int prenosA = 0;
  19. static int prenosB = 0;
  20. static int KOT_45 = 210;
  21. static int KOT_45_POL = 105;
  22. static const int BUF_AMOUNT = KOT_45 + KOT_45;
  23.  
  24. static const bool REVERSE = false;
  25. static const bool PORAVNAVA_NA_CRTO = true;
  26. static const int LIGHT_DIFF = 50;
  27. static const int DIRECTION_FORWARD = -1;
  28. static const int LIGHT_CHECKPOINT_DISTANCE = 0; // kdaj preverjamo
  29. static const int LIGHT_SENSORS_CHECKPOINT = 20; // manj od tolko da zabelezimo checkpoint
  30. static int wait4me = false;
  31.  
  32. static int checkpoints[20];
  33. static int rotA[CAPACITY];
  34. static int rotB[CAPACITY];
  35. static int currentPositionPlay = 0;
  36.  
  37. void init()
  38. {
  39. //nMaxRegulatedSpeedNxt = 750;
  40. //nMaxRegulatedSpeed12V = 750;
  41. nMotorPIDSpeedCtrl[motorA] = mtrSpeedReg;
  42. if (REVERSE) bMotorFlippedMode[motorA]=true;
  43. motor[motorA]=100;
  44. motor[motorA]=0;
  45. nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
  46. if (REVERSE) bMotorFlippedMode[motorB]=true;
  47. motor[motorB]=100;
  48. motor[motorB]=0;
  49. }
  50.  
  51.  
  52. void print(int i, int linenr, int wait=1000)
  53. {
  54. nxtDisplayBigTextLine(linenr, "%d ", i);
  55. wait1Msec(wait);
  56. }
  57. void print(int i, int i2, int linenr, int wait)
  58. {
  59. nxtDisplayBigTextLine(linenr, "%d %d ", i, i2);
  60. wait1Msec(1000);
  61. }
  62. int deli(int r, int cifra, int cifrapol)
  63. {
  64. int res = 0;
  65. int pz = 1;
  66. if (r < 0) {
  67. pz = -1;
  68. r = -r;
  69. }
  70. while (r > cifra) {
  71. res++;
  72. r -= cifra;
  73. }
  74. if (r > cifrapol)
  75. res++;
  76. return pz*res;
  77. }
  78.  
  79. void trackBoth()
  80. {
  81. int i = 0;
  82. int smer = 0;
  83.  
  84.  
  85. nMotorEncoder[motorA]=0;
  86. nMotorEncoder[motorB]=0;
  87. int bufA = 0;
  88. int bufB = 0;
  89. int checkpointIdx = 0;
  90.  
  91. int nothing_i = SPREMEMBA_SMERI_MOGOCE+1;
  92.  
  93. for (; i < CAPACITY; )
  94. {
  95. nxtDisplayBigTextLine(4, "I:%d ", i);
  96. wait1Msec(READ_WAIT);
  97.  
  98. bool isNothing = ((nMotorEncoder[motorA] == 0) && (nMotorEncoder[motorB] == 0));
  99. if (isNothing)
  100. {
  101. nothing_i++;
  102. }
  103.  
  104. if ((smer == 1 || smer == 2)) {
  105. int x;
  106. if (smer == 1)
  107. x = bufA;
  108. else
  109. x = bufB;
  110. x = deli(x, KOT_45, KOT_45_POL);
  111. x = x * 45;
  112. nxtDisplayBigTextLine(2, "KOT:%d* ", x);
  113. }
  114.  
  115. if (nothing_i >= SPREMEMBA_SMERI_MOGOCE) {
  116. nxtDisplayBigTextLine(0, "SMER? ");
  117. PlaySound(soundBlip);
  118. }
  119.  
  120. if (SensorValue(lightS1) < LIGHT_SENSORS_CHECKPOINT && SensorValue(lightS2) < LIGHT_SENSORS_CHECKPOINT)
  121. {
  122. nxtDisplayBigTextLine(6, "CHK%d ", checkpointIdx);
  123.  
  124. if (checkpointIdx < 1)
  125. checkpoints[checkpointIdx++] = i;
  126. else if (checkpoints[checkpointIdx-1] != i)
  127. checkpoints[checkpointIdx++] = i;
  128.  
  129. wait1Msec(1000);
  130. nxtDisplayBigTextLine(6, " ");
  131. }
  132.  
  133. if (isNothing && (SensorValue(touchS) == 0)) continue;
  134.  
  135. if ((nothing_i >= SPREMEMBA_SMERI_MOGOCE) || (abs(bufA) >= BUF_AMOUNT) || (abs(bufB) >= BUF_AMOUNT) || ((SensorValue(touchS) != 0) && (i > 0)))
  136. {
  137. if (bufA != 0 || bufB != 0)
  138. {
  139. // 470 = pravi kot
  140. // 235 = 45*
  141. if (smer == 1) {
  142. bufA = deli(bufA, KOT_45, KOT_45_POL) * KOT_45;
  143. rotA[i] = bufA;
  144. rotB[i] = 0;
  145. wait1Msec(1000);
  146. }
  147. else if (smer == 2) {
  148. bufB = deli(bufB, KOT_45, KOT_45_POL) * KOT_45;
  149. rotA[i] = 0;
  150. rotB[i] = bufB;
  151. }
  152. else if (smer == 0) {
  153. rotA[i] = bufA;
  154. rotB[i] = bufA;
  155. }
  156. i++;
  157. bufA = 0;
  158. bufB = 0;
  159. }
  160. }
  161. if (SensorValue(touchS) != 0) break;
  162. if (!isNothing && nothing_i >= SPREMEMBA_SMERI_MOGOCE)
  163. {
  164. nothing_i = 0;
  165. wait1Msec(READ_WAIT*10);
  166. int rA = abs(nMotorEncoder[motorA]);
  167. int rB = abs(nMotorEncoder[motorB]);
  168. if ((rA / 3) > rB) // obracanje
  169. {
  170. smer = 1; // desno
  171. } else if ((rB / 3) > rA) // obracanje
  172. {
  173. smer = 2; // levo
  174. } else {
  175. smer = 0;
  176. }
  177. if (smer == 0)
  178. nxtDisplayBigTextLine(0, "FORW ");
  179. else if (smer == 1)
  180. nxtDisplayBigTextLine(0, "LEFT ");
  181. else if (smer == 2)
  182. nxtDisplayBigTextLine(0, "RIGHT ");
  183. }
  184.  
  185. bufA += nMotorEncoder[motorA];
  186. bufB += nMotorEncoder[motorB];
  187. nMotorEncoder[motorA]=0;
  188. nMotorEncoder[motorB]=0;
  189. }
  190. if (i > 1) // remember before
  191. RecLength = i;
  192. }
  193.  
  194. void waitMotor(int motorN, int rot)
  195. {
  196. if (rot < 0)
  197. { // max 5s
  198. for (int i=0; i<500 && nMotorEncoder[motorN] > rot; i++) {
  199. wait1Msec(10);
  200. if (wait4me) {
  201. while (wait4me) { abortTimeslice(); }
  202. }
  203. }
  204. } else if (rot > 0)
  205. {
  206. for (int i=0; i<500 && nMotorEncoder[motorN] < rot; i++) {
  207. wait1Msec(10);
  208. if (wait4me) {
  209. while (wait4me) { abortTimeslice(); }
  210. }
  211. }
  212. }
  213. }
  214.  
  215. void getMotorDiff(int rotA, int rotB)
  216. {
  217. prenosA = rotA - nMotorEncoder[motorA];
  218. prenosB = rotB - nMotorEncoder[motorB];
  219. nMotorEncoder[motorA] = 0;
  220. nMotorEncoder[motorB] = 0;
  221. }
  222.  
  223. void driveMotor(int motorN, int rot)
  224. {
  225. if (rot == 0) return;
  226. if (rot < 0) {
  227. if (motorN == (int) motorA) {
  228. nMotorEncoder[motorN] = rot + prenosA;
  229. } else {
  230. nMotorEncoder[motorN] = rot + prenosB;
  231. }
  232. nMotorEncoderTarget[motorN] = 0;
  233. motor[motorN] = -SPEED_A;
  234. } else {
  235. nMotorEncoder[motorN] = 0;
  236. if (motorN == (int) motorA) {
  237. nMotorEncoderTarget[motorN] = -rot - prenosA;
  238. } else {
  239. nMotorEncoderTarget[motorN] = -rot - prenosB;
  240. }
  241. motor[motorN] = SPEED_A;
  242. }
  243. prenosA = 0;
  244. prenosB = 0;
  245. }
  246.  
  247. void driveBoth() {
  248. for(currentPositionPlay = 0; currentPositionPlay < RecLength; currentPositionPlay++){
  249. nxtDisplayBigTextLine(2, "I:%d ", currentPositionPlay);
  250. int rA = rotA[currentPositionPlay];
  251. int rB = rotB[currentPositionPlay];
  252. nSyncedMotors = synchNone;
  253.  
  254. if (rA == rB && rA == 0) continue;
  255. if (abs(rA) > 0 && abs(rB) > 0) {
  256. driveMotor(motorA, rA);
  257. driveMotor(motorB, rA);
  258. waitMotor(motorA, rA);
  259. getMotorDiff(rA, rA);
  260. } else if (abs(rA) > 0) {
  261. driveMotor(motorA, rA);
  262. waitMotor(motorA, rA);
  263. getMotorDiff(rA, rB);
  264. } else {
  265. driveMotor(motorB, rB);
  266. waitMotor(motorB, rB);
  267. getMotorDiff(rA, rB);
  268. }
  269. motor[motorA] = 0;
  270. motor[motorB] = 0;
  271. }
  272. nSyncedMotors = synchNone;
  273. motor[motorA] = 0;
  274. motor[motorB] = 0;
  275. }
  276.  
  277. void controlFloatMotors(bool val)
  278. {
  279. bFloatDuringInactiveMotorPWM=val;
  280. motor[motorA] = 100;
  281. motor[motorB] = 100;
  282. motor[motorA] = 0;
  283. motor[motorB] = 0;
  284. }
  285.  
  286. void blinkLights(int time)
  287. {
  288. SensorType[lightS1] = sensorLightActive;
  289. SensorType[lightS2] = sensorLightActive;
  290. wait1Msec(time);
  291. SensorType[lightS1] = sensorLightInactive;
  292. SensorType[lightS2] = sensorLightInactive;
  293. }
  294.  
  295. task poravnava()
  296. {
  297. wait1Msec(500);
  298. while (true)
  299. {
  300. int v1 = SensorValue(lightS1);
  301. int v2 = SensorValue(lightS2);
  302. bool checking = false;
  303. for (int i=0; i<20; i++)
  304. {
  305. if (checkpoints[i] == -100) break;
  306. if (checkpoints[i] >= (currentPositionPlay-LIGHT_CHECKPOINT_DISTANCE) &&
  307. checkpoints[i] <= (currentPositionPlay+LIGHT_CHECKPOINT_DISTANCE))
  308. {
  309. checking = true;
  310. }
  311. }
  312. if (checking
  313. && (abs(SensorValue(lightS1)-SensorValue(lightS2)) > LIGHT_DIFF))
  314. {
  315. hogCPU();
  316. wait4me = true;
  317.  
  318. int cacM1 = motor[motorA];
  319. int cacM2 = motor[motorB];
  320. motor[motorA] = 0;
  321. motor[motorB] = 0;
  322.  
  323. int motorN = motorA;
  324. int motorO = motorB;
  325. if (v1 < v2) {
  326. motorN = motorB;
  327. motorO = motorA;
  328. }
  329.  
  330. int cacME1 = nMotorEncoder[motorA];
  331. int cacMET1 = nMotorEncoderTarget[motorA];
  332. int cacME2 = nMotorEncoder[motorB];
  333. int cacMET2 = nMotorEncoderTarget[motorB];
  334.  
  335. blinkLights(100);
  336.  
  337. nMotorEncoderTarget[motorN] = 0;
  338. motor[motorN] = SPEED_LINE * DIRECTION_FORWARD;
  339. nMotorEncoderTarget[motorO] = 0;
  340. motor[motorO] = -SPEED_LINE * DIRECTION_FORWARD;
  341. nMotorEncoder[motorA] = 0;
  342. nMotorEncoder[motorB] = 0;
  343. while (abs(SensorValue(lightS1)-SensorValue(lightS2)) > LIGHT_DIFF) {
  344. nxtDisplayBigTextLine(6, "DIFF:%d ", abs(SensorValue(lightS1)-SensorValue(lightS2)));
  345. wait1Msec(50);
  346. }
  347. nxtDisplayBigTextLine(6, " ");
  348.  
  349. nMotorEncoder[motorA] = cacME1;
  350. nMotorEncoderTarget[motorA] = cacMET1;
  351. motor[motorA] = cacM1;
  352. nMotorEncoder[motorB] = cacME2;
  353. nMotorEncoderTarget[motorB] = cacMET2;
  354. motor[motorB] = cacM2;
  355.  
  356. wait4me = false;
  357. releaseCPU();
  358. blinkLights(100);
  359. wait1Msec(1000);
  360. }
  361. wait1Msec(100);
  362. }
  363. }
  364.  
  365. task main()
  366. {
  367. nxtDisplayBigTextLine(0, "INIT ");
  368.  
  369. for(int i=0; i<CAPACITY; i++){
  370. rotA[i]=0;
  371. rotB[i]=0;
  372. if (i < 20) checkpoints[i] = -100;
  373. }
  374.  
  375. init();
  376.  
  377. RecLength = 1;
  378. rotA[0] = -1200;
  379. rotB[0] = -1200;
  380.  
  381. wait1Msec(500);
  382.  
  383. while (true) {
  384. controlFloatMotors(true);
  385. wait1Msec(500);
  386. nxtDisplayBigTextLine(0, "RDY ");
  387. while(SensorValue(touchS) == 0);
  388. nxtDisplayBigTextLine(0, "WAIT ");
  389. wait1Msec(500);
  390. nxtDisplayBigTextLine(0, "REC ");
  391.  
  392. trackBoth();
  393. nxtDisplayBigTextLine(4, " ");
  394. nxtDisplayBigTextLine(6, " ");
  395.  
  396. nxtDisplayBigTextLine(0, "WAIT ");
  397. wait1Msec(500);
  398. nxtDisplayBigTextLine(0, "RDY ");
  399. while(SensorValue(touchS)==0);
  400. nxtDisplayBigTextLine(0, "PLAY ");
  401. controlFloatMotors(false);
  402. wait1Msec(1000);
  403.  
  404. if (PORAVNAVA_NA_CRTO) StartTask(poravnava);
  405. driveBoth();
  406. if (PORAVNAVA_NA_CRTO) StopTask(poravnava);
  407.  
  408. nxtDisplayBigTextLine(0, "DONE ");
  409. while(SensorValue(touchS)==0);
  410. nxtDisplayBigTextLine(0, "WAIT ");
  411. }
  412. }
Add Comment
Please, Sign In to add comment
Public Pastes
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!