API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 19th, 2018
70
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 30.87 KB | None | 0 0
raw download clone embed print report
  1. /* Simple skeletontest program for in-robot API */
  2.  
  3. #include "allcode_api.h"
  4. #include <stdlib.h>
  5.  
  6. #define NORMAL_SPEED 20
  7. #define SENSOR_STATE 0
  8. #define DRIVE_STATE 1
  9. #define WALL_STATE 2
  10. #define LIGHT_STATE 3
  11. #define LINE_STATE 4
  12.  
  13. #define WALL_CELL 1
  14. #define PASSAGE_CELL 2
  15. #define UNKNOWN_CELL 3
  16.  
  17.  
  18. //create a function to turn "90" degrees that checks the sensor for the direction its turning, should see a decrease then increase until it starts going back down again you're aligned
  19.  
  20. /*
  21. r/l 90d code needs a bit of work (could turn into function because 90*2 = 185, maybe a theme :thinkingemoiji:)
  22. FA_ResetEncoders();
  23. int l = FA_ReadEncoder(0);
  24. int r = FA_ReadEncoder(1);
  25. FA_LCDClear();
  26. set_motors(NORMAL_SPEED, -NORMAL_SPEED);
  27. while((FA_ReadEncoder(0) < l+185) && (FA_ReadEncoder(1) < r+185)) {
  28. //checking code for sensors (a way to prevent locking)
  29. }
  30. set_motors(0, 0);
  31. */
  32.  
  33. /*FA_LCDClear();
  34. FA_LCDPrint("Right:", 15, 5, 0, FONT_NORMAL, LCD_OPAQUE);
  35. FA_LCDNumber(FA_ReadIR(IR_RIGHT), 60, 0, FONT_NORMAL, LCD_OPAQUE);
  36. FA_LCDPrint("Left:", 15, 5, 15, FONT_NORMAL, LCD_OPAQUE);
  37. FA_LCDNumber(FA_ReadIR(IR_LEFT), 60, 15, FONT_NORMAL, LCD_OPAQUE);
  38. FA_DelayMillis(200);*/
  39.  
  40.  
  41. typedef struct cell_information {
  42. int wall_north;
  43. int wall_south;
  44. int wall_east;
  45. int wall_west;
  46. } cell_info;
  47.  
  48. typedef struct cell {
  49. int state;
  50. int visited;
  51. int direction_entered;
  52. cell_info *landmark;
  53. } cell;
  54.  
  55. cell map[4][4];
  56. int currentLoc[2];
  57.  
  58. void drive();
  59. void readSensors();
  60. void followWall();
  61. void set_speed(int active, int left, int right);
  62. void idle(int behav);
  63. void set_motors(int left, int right);
  64. void detectLight();
  65. void detectLine();
  66. void drive_straight(int behav, int left, int right);
  67. void turnLeftEnc(int behav, int enc);
  68.  
  69. void validateCell();
  70. void setupCells();
  71. void resetCell(int x, int y);
  72. void logCell();
  73. void straightenUp();
  74. void print_cell_info_debug();
  75. void moveToCenter();
  76. void updateLoc();
  77. void updateDirections();
  78. void explore();
  79. void print_cell();
  80.  
  81. void setEncSpeed(int param, int enc, int speed);
  82.  
  83. int wheelTicks[2];
  84. //TODO
  85. int motorTPMS[2];
  86.  
  87. int call = 0;
  88.  
  89. int encSpeed[2];
  90.  
  91. MX_UINT32 encTime;
  92.  
  93. int active[5];
  94. int left_speed[5];
  95. int right_speed[5];
  96. int a = 0;
  97. int d = 0;
  98. int direction = 0;
  99.  
  100. int time = 0;
  101.  
  102. int * directions;
  103.  
  104. int firstCell = 1;
  105.  
  106. int drivecounter = 0;
  107.  
  108. int oldspeed[2], newspeed[2];
  109.  
  110. void getSensorData();
  111.  
  112. int sensors[2];
  113.  
  114. MX_UINT32 sensorTime;
  115. int latch = 0;
  116.  
  117. void getSensorData() {
  118. if(!latch) {
  119. sensorTime = FA_ClockMS();
  120. latch = 1;
  121. }
  122. if(FA_ClockMS() >= sensorTime+5) {
  123. sensors[0] = FA_ReadIR(IR_LEFT);
  124. sensors[1] = FA_ReadIR(IR_RIGHT);
  125. }
  126. }
  127.  
  128. int main()
  129. {
  130. FA_RobotInit();
  131. FA_CompassInit();
  132. FA_LCDBacklight(50);
  133. setupCells();
  134. currentLoc[0] = 0;
  135. currentLoc[1] = 0;
  136. validateCell();
  137. //print_cell_info_debug();
  138. oldspeed[0] = 0;
  139. oldspeed[1] = 0;
  140. encSpeed[0] = 20;
  141. encSpeed[1] = 20;
  142. /*if(!call) {
  143. FA_SetMotors(20,20);
  144. FA_ResetEncoders();
  145. set_speed(WALL_STATE, encSpeed[0], encSpeed[1]);
  146. wheelTicks[0] = FA_ReadEncoder(0);
  147. wheelTicks[1] = FA_ReadEncoder(1);
  148. encTime = FA_ClockMS();
  149. call = 1;
  150. }
  151. if(FA_ClockMS() > encTime+50) {
  152. FA_LCDClear();
  153. FA_LCDNumber(wheelTicks[0], 20, 0, FONT_NORMAL, LCD_OPAQUE);
  154. FA_LCDNumber(wheelTicks[1], 40, 0, FONT_NORMAL, LCD_OPAQUE);
  155. FA_LCDNumber(FA_ReadEncoder(0), 20, 15, FONT_NORMAL, LCD_OPAQUE);
  156. FA_LCDNumber(FA_ReadEncoder(1), 40, 15, FONT_NORMAL, LCD_OPAQUE);
  157. call = 0;
  158. FA_SetMotors(0,0);
  159. FA_DelayMillis(1000);
  160. }*/
  161.  
  162. while(1)
  163. {
  164. //FA_LCDPrint("Right:", 15, 5, 0, FONT_NORMAL, LCD_OPAQUE);
  165. //FA_LCDNumber(FA_ReadIR(IR_RIGHT), 60, 0, FONT_NORMAL, LCD_OPAQUE);
  166. //FA_LCDPrint("Left:", 15, 5, 15, FONT_NORMAL, LCD_OPAQUE);
  167. //FA_LCDNumber(FA_ReadIR(IR_LEFT), 60, 15, FONT_NORMAL, LCD_OPAQUE);
  168. //FA_DelayMillis(200);
  169. //FA_DelayMillis(2000);
  170. /*if(FA_ReadIR(IR_RIGHT) > 450) {
  171. straightenUp(1);
  172. } else if(FA_ReadIR(IR_LEFT) > 450 ) {
  173. straightenUp(0);
  174. }*/
  175.  
  176. /*if((FA_ReadIR(IR_RIGHT) > 200) && (FA_ReadIR(IR_LEFT) > 200)) {
  177. if(((FA_ReadIR(IR_RIGHT) >= 340) && (FA_ReadIR(IR_RIGHT) <= 360)) && ((FA_ReadIR(IR_LEFT) >= 340) && (FA_ReadIR(IR_LEFT) <= 360))) {
  178. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  179. } else if(FA_ReadIR(IR_LEFT) > 700) {
  180. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10); //+15 -10 relies on left
  181. } else if(FA_ReadIR(IR_RIGHT) > 700) {
  182. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  183. } else if(FA_ReadIR(IR_RIGHT) > 500) {
  184. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  185. } else if(FA_ReadIR(IR_LEFT) > 500) {
  186. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10);
  187. } else if(FA_ReadIR(IR_RIGHT) > 360) {
  188. set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED+10);
  189. } else if(FA_ReadIR(IR_LEFT) > 360) {
  190. set_speed(WALL_STATE, NORMAL_SPEED+10, NORMAL_SPEED-10);
  191. } else if(FA_ReadIR(IR_RIGHT) < 340) {
  192. set_speed(WALL_STATE, NORMAL_SPEED+10, NORMAL_SPEED-10);
  193. } else if(FA_ReadIR(IR_LEFT) < 340) {
  194. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  195. } else if(FA_ReadIR(IR_RIGHT) < 280) {
  196. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10);
  197. } else if(FA_ReadIR(IR_LEFT) < 280) {
  198. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+15);
  199. } else {
  200. set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  201. }
  202. } else */
  203. getSensorData();
  204. /*if((sensors[1] > 200) && (FA_ReadIR(IR_LEFT) > 200)) {
  205. if(((sensors[1] >= 340) && (sensors[1] <= 360)) && ((sensors[0] >= 340) && (sensors[0] <= 360))) {
  206. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  207. } else if(sensors[0] > 700) {
  208. setEncSpeed(1,3,5);
  209. } else if(sensors[1] > 700) {
  210. setEncSpeed(0,2,5);
  211. } else if(sensors[1] > 500) {
  212. setEncSpeed(0,2,5);
  213. } else if(sensors[0] > 500) {
  214. setEncSpeed(1,3,5);
  215. } else if(sensors[1] > 360) {
  216. setEncSpeed(0,1,5);
  217. } else if(sensors[0] > 360) {
  218. setEncSpeed(1,2,5);
  219. } else if(sensors[1] < 280) { //one of these vals is wrong, left side works fine right doesn't maybe make all calls to turn the left wheel faster speed*1.5 since left wheel is bad
  220. setEncSpeed(1,3,5);
  221. } else if(sensors[0] < 280) {
  222. setEncSpeed(0,2,5);
  223. } else if(sensors[1] < 340) {
  224. setEncSpeed(1,3,5);
  225. } else if(sensors[0] < 340) {
  226. setEncSpeed(0,2,5);
  227. }
  228. } else if(sensors[1] > 200) {
  229. if(sensors[1] > 700) {
  230. setEncSpeed(0,2,5);
  231. } else if(sensors[1] > 500) {
  232. setEncSpeed(0,2,5);
  233. } else if(sensors[1] > 360) {
  234. setEncSpeed(0,1,5);
  235. } else if((sensors[1] >= 340) && (sensors[1] <= 360)) {
  236. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  237. } else if(sensors[1] < 280) {
  238. setEncSpeed(1,3,5);
  239. } else if(sensors[1] < 340) {
  240. setEncSpeed(1,3,5);
  241. } else {
  242. //set_speed(WALL_STATE, NORMAL_SPEED+10, NORMAL_SPEED);
  243. }
  244. } else if(sensors[0] > 200) {
  245. if(sensors[0] > 700) {
  246. setEncSpeed(1,3,5);
  247. } else if(sensors[0] > 500) {
  248. setEncSpeed(1,3,5);
  249. } else if(sensors[0] > 360) {
  250. setEncSpeed(1,1,10);
  251. } else if((sensors[0] >= 340) && (sensors[0] <= 360)) { //don't change anything below this, right side good
  252. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  253. } else if(sensors[0] < 280) {
  254. setEncSpeed(0,2,5);
  255. } else if(sensors[0] < 340) {
  256. setEncSpeed(0,2,5);
  257. }
  258. } else {
  259. idle(WALL_STATE);
  260. }*/
  261. //} else {
  262. // set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  263. //set_speed(WALL_STATE, 0,0);
  264. //}
  265.  
  266. print_cell();
  267.  
  268. detectLine();
  269. detectLight();
  270. readSensors();
  271. followWall();
  272. drive();
  273.  
  274. if(active[LIGHT_STATE]) {
  275. newspeed[0] = left_speed[LIGHT_STATE];
  276. newspeed[1] = right_speed[LIGHT_STATE];
  277. if((newspeed[0] != oldspeed[0]) || (newspeed[1] != oldspeed[1])) {
  278. FA_SetMotors(newspeed[0], newspeed[1]);
  279. oldspeed[0] = newspeed[0];
  280. oldspeed[1] = newspeed[1];
  281. FA_ResetEncoders();
  282. }
  283. } else if(active[LINE_STATE]) {
  284. validateCell();
  285. //print_cell_info_debug();
  286. } else if(active[SENSOR_STATE]) {
  287. if(d) {
  288. FA_Right(a);
  289. } else {
  290. FA_Left(a);
  291. }
  292. /*if(FA_ReadIR(IR_LEFT) > 400) {
  293. straightenUp(0);
  294. } else if(FA_ReadIR(IR_RIGHT) > 400){
  295. straightenUp(1);
  296. }*/
  297. oldspeed[0] = 0;
  298. oldspeed[1] = 0;
  299. encSpeed[0] = NORMAL_SPEED;
  300. encSpeed[1] = NORMAL_SPEED;
  301. call = 0;
  302. FA_ResetEncoders();
  303. } else if(active[WALL_STATE]) {
  304. newspeed[0] = left_speed[WALL_STATE];
  305. newspeed[1] = right_speed[WALL_STATE];
  306. if((newspeed[0] != oldspeed[0]) || (newspeed[1] != oldspeed[1])) {
  307. FA_SetMotors(newspeed[0], newspeed[1]);
  308. oldspeed[0] = newspeed[0];
  309. oldspeed[1] = newspeed[1];
  310. FA_ResetEncoders();
  311. }
  312. } else if(active[DRIVE_STATE]) {
  313. newspeed[0] = left_speed[DRIVE_STATE];
  314. newspeed[1] = right_speed[DRIVE_STATE];
  315. if((newspeed[0] != oldspeed[0]) || (newspeed[1] != oldspeed[1])) {
  316. FA_SetMotors(newspeed[0], newspeed[1]);
  317. oldspeed[0] = newspeed[0];
  318. oldspeed[1] = newspeed[1];
  319. FA_ResetEncoders();
  320. }
  321. }
  322. }
  323. return 0;
  324. }
  325.  
  326. void set_motors(int left, int right) {
  327. FA_SetMotors(left, right);
  328. }
  329.  
  330. void drive() {
  331. drive_straight(DRIVE_STATE, NORMAL_SPEED, NORMAL_SPEED);
  332. }
  333.  
  334. void readSensors() {
  335. /*
  336. obstacle avoidence reactive code, hierarchy = front > right > left
  337. */
  338. if(FA_ReadIR(IR_FRONT) > 650) {
  339. if((FA_ReadIR(IR_RIGHT) > 200) && //380
  340. (FA_ReadIR(IR_LEFT) > 200)) { //550
  341. a = 90;
  342. d = 0;
  343. active[SENSOR_STATE] = 1;
  344. direction -= 1;
  345. if(direction == 4) direction = 0;
  346. //set_speed(SENSOR_STATE, -NORMAL_SPEED, -NORMAL_SPEED); //if front, left and right detected, replace with turn 180
  347. } else if(FA_ReadIR(IR_RIGHT) > 200) {
  348. a = 90;
  349. d = 0;
  350. active[SENSOR_STATE] = 1;
  351. direction -= 1;
  352. if(direction == -1) direction = 3;
  353. } else if(FA_ReadIR(IR_LEFT) > 200) {
  354. a = 90;
  355. d = 1;
  356. active[SENSOR_STATE] = 1;
  357. direction += 1;
  358. if(direction == 4) direction = 0;
  359. } else if(FA_ReadIR(IR_FRONT_RIGHT) > 1800) { //if front left detected
  360. //set_speed(SENSOR_STATE, -NORMAL_SPEED, NORMAL_SPEED); //replace with 90 degree left
  361. a = 90;
  362. d = 0;
  363. active[SENSOR_STATE] = 1;
  364. direction -= 1;
  365. if(direction == 4) direction = 0;
  366. } else if(FA_ReadIR(IR_FRONT_LEFT) > 1800) { //if front right detected
  367. //set_speed(SENSOR_STATE, NORMAL_SPEED, -NORMAL_SPEED); //replace with 90 degree right
  368. a = 90;
  369. d = 1;
  370. active[SENSOR_STATE] = 1;
  371. direction += 1;
  372. if(direction == -1) direction = 3;
  373. } else {
  374. //set_speed(SENSOR_STATE, NORMAL_SPEED, -NORMAL_SPEED); //opt for taking left > right, replace with 90 degree left
  375. a = 90;
  376. d = 0;
  377. active[SENSOR_STATE] = 1;
  378. direction -= 1;
  379. if(direction == 4) direction = 0;
  380. }
  381. /*} else if(FA_ReadIR(IR_FRONT_RIGHT) > 1800) {
  382. //set_speed(SENSOR_STATE, -NORMAL_SPEED, NORMAL_SPEED); //sharp turn left, could be made better
  383. a = 90;
  384. d = 0;
  385. active[SENSOR_STATE] = 1;
  386. direction += 1;
  387. if(direction == 4) direction = 0;
  388. } else if(FA_ReadIR(IR_FRONT_LEFT) > 1800) {
  389. //set_speed(SENSOR_STATE, NORMAL_SPEED, -NORMAL_SPEED); //sharp turn right, could be made better
  390. a = 90;
  391. d = 1;
  392. active[SENSOR_STATE] = 1;
  393. direction -= 1;
  394. if(direction == -1) direction = 3;*/
  395. } else {
  396. idle(SENSOR_STATE);
  397. }
  398. }
  399.  
  400. void detectLight() {
  401. //if(FA_ReadLight() < 150) { //if it's a shady boi stop before you get robbed
  402. // set_speed(LIGHT_STATE, 0, 0);
  403. //} else {
  404. // idle(LIGHT_STATE);
  405. //}
  406. }
  407.  
  408. void detectLine() {
  409. if((FA_ReadLine(1) < 50) && (FA_ReadLine(0) < 50)) {
  410. active[LINE_STATE] = 1;
  411. } else {
  412. idle(LINE_STATE);
  413. }
  414. }
  415.  
  416. void turnLeftEnc(int behav, int enc) { //make right encoder turn faster than left
  417. if(FA_ClockMS() > time+5) {
  418. if(FA_ReadEncoder(1) > FA_ReadEncoder(0)+enc) {
  419. drive_straight(behav, NORMAL_SPEED, NORMAL_SPEED);
  420. } else if(FA_ReadEncoder(0)+enc > FA_ReadEncoder(1)){
  421. set_speed(behav, NORMAL_SPEED, NORMAL_SPEED+10);
  422. }
  423. time = FA_ClockMS();
  424. }
  425. }
  426.  
  427. void followWall() {
  428. if((FA_ReadIR(IR_RIGHT) > 200) && (FA_ReadIR(IR_LEFT) > 200)) {
  429. if(((FA_ReadIR(IR_RIGHT) >= 340) && (FA_ReadIR(IR_RIGHT) <= 360)) && ((FA_ReadIR(IR_LEFT) >= 340) && (FA_ReadIR(IR_LEFT) <= 360))) {
  430. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  431. } else if(FA_ReadIR(IR_LEFT) > 700) {
  432. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-15); //+15 -10 relies on left
  433. } else if(FA_ReadIR(IR_RIGHT) > 700) {
  434. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  435. } else if(FA_ReadIR(IR_RIGHT) > 500) {
  436. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  437. } else if(FA_ReadIR(IR_LEFT) > 500) {
  438. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10);
  439. } else if(FA_ReadIR(IR_RIGHT) > 360) {
  440. set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED+10);
  441. } else if(FA_ReadIR(IR_LEFT) > 360) {
  442. set_speed(WALL_STATE, NORMAL_SPEED+10, NORMAL_SPEED-10);
  443. } else if(FA_ReadIR(IR_RIGHT) < 340) {
  444. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10);
  445. } else if(FA_ReadIR(IR_LEFT) < 340) {
  446. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  447. } else if(FA_ReadIR(IR_RIGHT) < 280) {
  448. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-15);
  449. } else if(FA_ReadIR(IR_LEFT) < 280) {
  450. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+15);
  451. } else {
  452. /*else favour leaning towards the right wall until other statements
  453. are triggered*/
  454. set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  455. }
  456. } else if(FA_ReadIR(IR_RIGHT) > 200) {
  457. if(FA_ReadIR(IR_RIGHT) > 700) {
  458. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  459. } else if(FA_ReadIR(IR_RIGHT) > 500) {
  460. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  461. } else if(FA_ReadIR(IR_RIGHT) > 360) {
  462. set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED+10);
  463. } else if((FA_ReadIR(IR_RIGHT) >= 340) && (FA_ReadIR(IR_RIGHT) <= 360)) {
  464. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  465. } else if(FA_ReadIR(IR_RIGHT) < 340) {
  466. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10);
  467. } else if(FA_ReadIR(IR_RIGHT) < 280) {
  468. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-15);
  469. } else {
  470. set_speed(WALL_STATE, NORMAL_SPEED+10, NORMAL_SPEED);
  471. }
  472. } else if(FA_ReadIR(IR_LEFT) > 200) {
  473. if(FA_ReadIR(IR_LEFT) > 700) {
  474. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-15); //maybe replace with a quik turn my dood
  475. } else if(FA_ReadIR(IR_LEFT) > 500) {
  476. set_speed(WALL_STATE, NORMAL_SPEED+15, NORMAL_SPEED-10);
  477. } else if(FA_ReadIR(IR_LEFT) > 360) {
  478. set_speed(WALL_STATE, NORMAL_SPEED+10, NORMAL_SPEED-10);
  479. } else if((FA_ReadIR(IR_LEFT) >= 340) && (FA_ReadIR(IR_LEFT) <= 360)) { //don't change anything below this, right side good
  480. drive_straight(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED);
  481. } else if(FA_ReadIR(IR_LEFT) < 340) {
  482. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+10);
  483. } else if(FA_ReadIR(IR_LEFT) < 280) {
  484. set_speed(WALL_STATE, NORMAL_SPEED-10, NORMAL_SPEED+15);
  485. } else {
  486. set_speed(WALL_STATE, NORMAL_SPEED, NORMAL_SPEED+10);
  487. }
  488. } else {
  489. idle(WALL_STATE);
  490. }
  491. }
  492.  
  493. //todo
  494.  
  495. void setEncSpeed(int param, int inc, int speed) {
  496. if(!call) {
  497. set_speed(WALL_STATE, encSpeed[0], encSpeed[1]);
  498. wheelTicks[0] = FA_ReadEncoder(0);
  499. wheelTicks[1] = FA_ReadEncoder(1);
  500. encTime = FA_ClockMS();
  501. call = 1;
  502. }
  503. if(FA_ClockMS() > encTime+30) {
  504. motorTPMS[0] = FA_ReadEncoder(0) - wheelTicks[0];
  505. motorTPMS[1] = FA_ReadEncoder(1) - wheelTicks[1];
  506. switch(param) {
  507. case 0:
  508. encSpeed[0] = NORMAL_SPEED;
  509. if (motorTPMS[1] > (motorTPMS[0] + inc)) {
  510. //going faster
  511. set_speed(WALL_STATE, encSpeed[0], encSpeed[1] -= speed);
  512. } else if (motorTPMS[1] == (motorTPMS[0]+inc)) {
  513. //turning at desired angle
  514. } else {
  515. set_speed(WALL_STATE, encSpeed[0], encSpeed[1] += speed);
  516. }
  517. break;
  518. case 1:
  519. encSpeed[1] = NORMAL_SPEED;
  520. if (motorTPMS[0] > (motorTPMS[1] + inc)) {
  521. set_speed(WALL_STATE, encSpeed[0]-=speed, encSpeed[1]); //make this faster or call with left faster than right, test it out but following is solid on one side
  522. } else if (motorTPMS[1] == (motorTPMS[0]+inc)) {
  523.  
  524. } else {
  525. set_speed(WALL_STATE, encSpeed[0]+=speed, encSpeed[1]);
  526. }
  527. break;
  528. default:
  529. break;
  530. }
  531. call = 0;
  532. }
  533. }
  534.  
  535. //create var to make sure it never sets the same speed twice, keep old speed and new speed etc
  536. void drive_straight(int behav, int left, int right) {
  537. if(drivecounter == 0) {
  538. if(FA_ReadEncoder(1) > FA_ReadEncoder(0)) {
  539. set_speed(behav, NORMAL_SPEED+10, NORMAL_SPEED);
  540. } else if(FA_ReadEncoder(0) > FA_ReadEncoder(1)){
  541. set_speed(behav, NORMAL_SPEED, NORMAL_SPEED+10);
  542. } else {
  543. set_speed(behav, NORMAL_SPEED, NORMAL_SPEED);
  544. }
  545. }
  546. drivecounter+=1;
  547. if(drivecounter == 10) {
  548. drivecounter = 0;
  549. }
  550. }
  551.  
  552. void set_speed(int behav, int left, int right) {
  553. left_speed[behav] = left;
  554. right_speed[behav] = right;
  555. active[behav] = 1;
  556. }
  557.  
  558. void idle(int behav) {
  559. active[behav] = 0;
  560. }
  561.  
  562. void setupCells() {
  563. int i, j;
  564. for (i = 0; i < 4; i++) {
  565. for (j = 0; j < 4; j++) {
  566. map[i][j].direction_entered = (int) NULL;
  567. map[i][j].visited = 0;
  568. map[i][j].state = 3;
  569. cell_info *temp = malloc(sizeof(cell_info));
  570. temp->wall_east = 0;
  571. temp->wall_west = 0;
  572. temp->wall_north = 0;
  573. temp->wall_south = 0;
  574. map[i][j].landmark = temp;
  575. }
  576. }
  577. }
  578.  
  579. void resetCell(int x, int y) {
  580. map[x][y].direction_entered = (int) NULL;
  581. map[x][y].visited = 0;
  582. map[x][y].state = 3;
  583. free(map[x][y].landmark);
  584. cell_info *temp = malloc(sizeof(cell_info));
  585. temp->wall_east = 0;
  586. temp->wall_west = 0;
  587. temp->wall_north = 0;
  588. temp->wall_south = 0;
  589. map[x][y].landmark = temp;
  590. }
  591.  
  592. void updateLoc() {
  593. int i;
  594. //this will be called after you've moved since the direction you're facing will be the direction you entered the cell at
  595. switch(direction) {
  596. case 0: //N
  597. if (currentLoc[0] == 0) { //shift everything down 1
  598. for (i = 0; i < 4; i++) {
  599. cell temp, temp2;
  600. temp = map[1][i];
  601. map[1][i] = map[0][i];
  602. resetCell(0, i); //wipe cell in future
  603. temp2 = map[2][i];
  604. map[2][i] = temp;
  605. map[3][i] = temp2;
  606. }
  607. currentLoc[0] += 1;
  608. }
  609. currentLoc[0] -= 1;
  610. break;
  611. case 1: //E
  612. if (currentLoc[1] == 3) { //shift everything left 1
  613. for (i = 0; i < 4; i++) {
  614. cell temp, temp2;
  615. temp = map[i][2];
  616. map[i][2] = map[i][3];
  617. resetCell(i, 3); //wipe cell in future
  618. temp2 = map[i][1];
  619. map[i][1] = temp;
  620. map[i][0] = temp2;
  621. }
  622. currentLoc[1] -= 1;
  623. }
  624. currentLoc[1] += 1;
  625. break;
  626. case 2: //S
  627. if (currentLoc[0] == 3) { //shift everything up 1
  628. for (i = 0; i < 4; i++) {
  629. cell temp, temp2;
  630. temp = map[2][i];
  631. map[2][i] = map[3][i];
  632. resetCell(3, i); //wipe cell in future
  633. temp2 = map[1][i];
  634. map[1][i] = temp;
  635. map[0][i] = temp2;
  636. }
  637. currentLoc[0] -= 1;
  638. }
  639. currentLoc[0] += 1;
  640. break;
  641. case 3: //W
  642. if (currentLoc[1] == 0) { //shift everything right 1
  643. for (i = 0; i < 4; i++) {
  644. cell temp, temp2;
  645. temp = map[i][1];
  646. map[i][1] = map[i][0];
  647. resetCell(i, 0); //wipe cell in future
  648. temp2 = map[i][2];
  649. map[i][2] = temp;
  650. map[i][3] = temp2;
  651. }
  652. currentLoc[1] += 1;
  653. }
  654. currentLoc[1] -= 1;
  655. break;
  656. default: //doesn't matter will never get here
  657. break;
  658. }
  659. if(map[currentLoc[0]][currentLoc[1]].visited) map[currentLoc[0]][currentLoc[1]].visited+=1;
  660. }
  661.  
  662. void logCell() {
  663. int numwalls = 0;
  664. cell tempcell;//think it's an issue here somewhere?
  665. tempcell.direction_entered = direction; //take current direction 0 = N, 1 = E, 2 = S, 3 = W
  666. tempcell.visited = 1; // set to visited since you've been there
  667. tempcell.landmark = malloc(sizeof(cell_info)); //malloc may cause issues on x16 compiler
  668. if(FA_ReadIR(directions[1]) > 200) {
  669. tempcell.landmark->wall_east = 1;
  670. numwalls+=1;
  671. } else {
  672. tempcell.landmark->wall_east = 0;
  673. }
  674. if(FA_ReadIR(directions[3]) > 200) { //not sure if west/east
  675. tempcell.landmark->wall_west = 1;
  676. numwalls+=1;
  677. } else {
  678. tempcell.landmark->wall_west = 0;
  679. }
  680. if(FA_ReadIR(directions[0]) > 200) {
  681. tempcell.landmark->wall_north = 1;
  682. numwalls+=1;
  683. } else {
  684. tempcell.landmark->wall_north = 0;
  685. }
  686. if(FA_ReadIR(directions[2]) > 200) {
  687. tempcell.landmark->wall_south = 1;
  688. numwalls+=1;
  689. } else {
  690. tempcell.landmark->wall_south = 0;
  691. }
  692. if(numwalls >= 2) {
  693. tempcell.state = 2;
  694. } else {
  695. tempcell.state = 1;
  696. }
  697. map[currentLoc[0]][currentLoc[1]] = tempcell;
  698. if(firstCell) firstCell = 0;
  699. }
  700.  
  701. void moveToCenter() {
  702. while((FA_ReadEncoder(0) < 290) && (FA_ReadEncoder(1) < 290)) {
  703.  
  704. followWall();
  705. drive();
  706.  
  707. if(active[WALL_STATE]) {
  708. newspeed[0] = left_speed[WALL_STATE];
  709. newspeed[1] = right_speed[WALL_STATE];
  710. if((newspeed[0] != oldspeed[0]) || (newspeed[1] != oldspeed[1])) {
  711. FA_SetMotors(newspeed[0], newspeed[1]);
  712. oldspeed[0] = newspeed[0];
  713. oldspeed[1] = newspeed[1];
  714. }
  715. } else if(active[DRIVE_STATE]) {
  716. newspeed[0] = left_speed[DRIVE_STATE];
  717. newspeed[1] = right_speed[DRIVE_STATE];
  718. if((newspeed[0] != oldspeed[0]) || (newspeed[1] != oldspeed[1])) {
  719. FA_SetMotors(newspeed[0], newspeed[1]);
  720. oldspeed[0] = newspeed[0];
  721. oldspeed[1] = newspeed[1];
  722. }
  723. }
  724. }
  725. }
  726.  
  727. void updateDirections() {
  728. directions = malloc(4*sizeof(int));
  729. switch(direction) {
  730. case 0: //n
  731. directions[0] = IR_FRONT;
  732. directions[1] = IR_RIGHT;
  733. directions[2] = IR_REAR;
  734. directions[3] = IR_LEFT;
  735. break;
  736. case 1: //e
  737. directions[0] = IR_LEFT;
  738. directions[1] = IR_FRONT;
  739. directions[2] = IR_RIGHT;
  740. directions[3] = IR_REAR;
  741. break;
  742. case 2: //s
  743. directions[0] = IR_REAR;
  744. directions[1] = IR_LEFT;
  745. directions[2] = IR_FRONT;
  746. directions[3] = IR_RIGHT;
  747. break;
  748. case 3: //w
  749. directions[0] = IR_RIGHT;
  750. directions[1] = IR_REAR;
  751. directions[2] = IR_LEFT;
  752. directions[3] = IR_FRONT;
  753. break;
  754. }
  755. }
  756.  
  757. void explore() {
  758. int lowest[2];
  759. lowest[0] = 100; //big starting number
  760. lowest[1] = 4;
  761. if(!map[currentLoc[0]][currentLoc[1]].landmark->wall_north) {
  762. if(map[currentLoc[0]-1][currentLoc[1]].visited < lowest[0]) { //north
  763. lowest[0] = map[currentLoc[0]-1][currentLoc[1]].visited;
  764. lowest[1] = 0; //north
  765. }
  766. }
  767. if(!map[currentLoc[0]][currentLoc[1]].landmark->wall_east) { //will have a problem with things out of bounds maybe?
  768. if(map[currentLoc[0]][currentLoc[1]+1].visited < lowest[0]) { //east
  769. lowest[0] = map[currentLoc[0]][currentLoc[1]+1].visited;
  770. lowest[1] = 1; //east
  771. }
  772. }
  773. if(!map[currentLoc[0]][currentLoc[1]].landmark->wall_south) {
  774. if(map[currentLoc[0]+1][currentLoc[1]].visited < lowest[0]) { //south
  775. lowest[0] = map[currentLoc[0]+1][currentLoc[1]].visited;
  776. lowest[1] = 2; //south
  777. }
  778. }
  779. if(!map[currentLoc[0]][currentLoc[1]].landmark->wall_west) {
  780. if(map[currentLoc[0]+1][currentLoc[1]-1].visited < lowest[0]) { //west
  781. lowest[0] = map[currentLoc[0]][currentLoc[1]-1].visited;
  782. lowest[1] = 3; //west
  783. }
  784. }
  785. /*FA_LCDClear();
  786. FA_LCDPrint("N:", 10, 10, 0, FONT_NORMAL, LCD_OPAQUE);
  787. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_north, 30, 0, FONT_NORMAL, LCD_OPAQUE);
  788. FA_LCDPrint("E:", 50, 50, 0, FONT_NORMAL, LCD_OPAQUE);
  789. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_east, 70, 0, FONT_NORMAL, LCD_OPAQUE);
  790. FA_LCDPrint("S:", 10, 10, 15, FONT_NORMAL, LCD_OPAQUE);
  791. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_south, 30, 15, FONT_NORMAL, LCD_OPAQUE);
  792. FA_LCDPrint("W:", 50, 50, 15, FONT_NORMAL, LCD_OPAQUE);
  793. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_west, 70, 15, FONT_NORMAL, LCD_OPAQUE);
  794. FA_DelayMillis(5000);*/
  795. if(direction > lowest[1]) {
  796. while(direction != lowest[1]) {
  797. FA_Left(90);
  798. direction-=1;
  799. }
  800. } else if(direction < lowest[1]) {
  801. while(direction != lowest[1]) {
  802. FA_Right(90);
  803. direction+=1;
  804. }
  805. }
  806. FA_PlayNote(400,100);
  807. }
  808.  
  809. void validateCell() {
  810. /*
  811. go to center of cell
  812. start state gathering to see what you want to do
  813. anaylze > decide where to go > straighten up final
  814. */
  815. //Getting to center and aligning with north code
  816. FA_ResetEncoders();
  817. if(!firstCell) {
  818. moveToCenter();
  819. updateLoc();
  820. }
  821. //checking code to see if you're in the center, check all walls and align accordingly
  822. //instead of turning change which sensors you're reading from so make it so you don't need to move around
  823. if(!map[currentLoc[0]][currentLoc[1]].visited) {
  824. updateDirections();
  825. logCell();
  826. }
  827. /*
  828. if(FA_ReadIR(IR_LEFT) > 400) {
  829. straightenUp(0);
  830. } else if(FA_ReadIR(IR_RIGHT) > 400){
  831. straightenUp(1);
  832. }
  833. */
  834. if(map[currentLoc[0]][currentLoc[1]].visited) {
  835. //use a currentlowest var xd
  836. explore();
  837.  
  838. /*if(FA_ReadIR(IR_LEFT) > 400) {
  839. straightenUp(0);
  840. } else if(FA_ReadIR(IR_RIGHT) > 400){
  841. straightenUp(1);
  842. }*/
  843. }
  844.  
  845. //Getting sensor data and creating/adding cell code
  846.  
  847. /*if(firstCell) {
  848. map[currentLoc[0]][currentLoc[1]] = tempcell;
  849. firstCell = 0;
  850. } else {
  851. addCell(tempcell);
  852. }
  853. if(direction > olddir) {
  854. while(direction != olddir) {
  855. FA_Left(90);
  856. direction-=1;
  857. }
  858. } else if(direction < olddir) {
  859. while(direction != olddir) {
  860. FA_Right(90);
  861. direction+=1;
  862. }
  863. }
  864. FA_DelayMillis(1000);
  865. oldspeed[0] = 0;
  866. oldspeed[1] = 0;*/
  867. }
  868.  
  869. void print_cell_info_debug() {
  870. FA_SetMotors(0,0);
  871. int opt = 0;
  872. while((FA_ReadSwitch(0) != 1) && (FA_ReadSwitch(1) != 1)) {
  873. FA_LCDClear();
  874. switch(opt) {
  875. case 0:
  876. FA_LCDPrint("N:", 15, 5, 0, FONT_NORMAL, LCD_OPAQUE);
  877. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_north, 20, 0, FONT_NORMAL, LCD_OPAQUE);
  878. break;
  879. case 1:
  880. FA_LCDPrint("E:", 15, 5, 0, FONT_NORMAL, LCD_OPAQUE);
  881. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_east, 20, 0, FONT_NORMAL, LCD_OPAQUE);
  882. break;
  883. case 2:
  884. FA_LCDPrint("S:", 15, 5, 0, FONT_NORMAL, LCD_OPAQUE);
  885. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_south, 20, 0, FONT_NORMAL, LCD_OPAQUE);
  886. break;
  887. case 3:
  888. FA_LCDPrint("W:", 15, 5, 0, FONT_NORMAL, LCD_OPAQUE);
  889. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].landmark->wall_west, 20, 0, FONT_NORMAL, LCD_OPAQUE);
  890. break;
  891. default:
  892. break;
  893. }
  894. FA_LCDPrint("Direction:", 15, 5, 15, FONT_NORMAL, LCD_OPAQUE);
  895. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].direction_entered, 70, 15, FONT_NORMAL, LCD_OPAQUE);
  896. FA_DelayMillis(500);
  897. opt+=1;
  898. if(opt == 4) opt = 0;
  899. }
  900. FA_LCDClear();
  901. }
  902.  
  903.  
  904.  
  905. /*
  906. go into reactive state machine when in line function to vote and decide on what to do (i.e turn for map follow, don't repeat scan squares etc)
  907. */
  908. void straightenUp(int walls) {
  909. int irval;
  910. long counter = 0;
  911. int flag = 0;
  912. int first = 1;
  913. switch(walls) {
  914. case 0: //left
  915. irval = FA_ReadIR(IR_LEFT);
  916. FA_SetMotors(NORMAL_SPEED, -NORMAL_SPEED);
  917. while(1) {
  918. if(counter == 100000) {
  919. if(first) {
  920. if(irval > (FA_ReadIR(IR_LEFT)+15)) {
  921. flag = 1;
  922. break;
  923. }
  924. }
  925. if(irval > (FA_ReadIR(IR_LEFT)+15)) break; //10 or 15 idk
  926. irval = FA_ReadIR(IR_LEFT);
  927. counter = 0;
  928. first = 0;
  929. }
  930. counter += 1;
  931. }
  932. if(flag) {
  933. counter = 0;
  934. FA_PlayNote(400,200);
  935. FA_SetMotors(-NORMAL_SPEED, NORMAL_SPEED);
  936. while(1) {
  937. if(counter == 100000) {
  938. if(irval < (FA_ReadIR(IR_LEFT)+15)) break; //10 or 15 idk
  939. irval = FA_ReadIR(IR_LEFT);
  940. counter = 0;
  941. }
  942. counter += 1;
  943. }
  944. }
  945. break;
  946. case 1: //right
  947. irval = FA_ReadIR(IR_RIGHT);
  948. FA_SetMotors(-NORMAL_SPEED, NORMAL_SPEED);
  949. while(1) {
  950. if(counter == 100000) {
  951. if(first) {
  952. if(irval > (FA_ReadIR(IR_RIGHT)+5)) {
  953. flag = 1;
  954. break;
  955. }
  956. }
  957. if(irval > (FA_ReadIR(IR_RIGHT)+5)) break; //10 or 15 idk
  958. irval = FA_ReadIR(IR_RIGHT);
  959. counter = 0;
  960. }
  961. counter += 1;
  962. first = 0;
  963. }
  964. if(flag) {
  965. counter = 0;
  966. FA_SetMotors(-NORMAL_SPEED, NORMAL_SPEED);
  967. while(1) {
  968. if(counter == 100000) {
  969. if(irval < (FA_ReadIR(IR_RIGHT)+5)) break; //10 or 15 idk
  970. irval = FA_ReadIR(IR_RIGHT);
  971. counter = 0;
  972. }
  973. counter += 1;
  974. }
  975. }
  976. break;
  977. }
  978. }
  979.  
  980. void print_cell() {
  981. FA_LCDClear();
  982. FA_LCDPrint("Y:", 60, 60, 0, FONT_NORMAL, LCD_OPAQUE);
  983. FA_LCDNumber(currentLoc[0], 70, 0, FONT_NORMAL, LCD_OPAQUE);
  984. FA_LCDPrint("X:", 80, 80, 0, FONT_NORMAL, LCD_OPAQUE);
  985. FA_LCDNumber(currentLoc[1], 90, 0, FONT_NORMAL, LCD_OPAQUE);
  986. FA_LCDPrint("Vis:", 80, 80, 15, FONT_NORMAL, LCD_OPAQUE);
  987. FA_LCDNumber(map[currentLoc[0]][currentLoc[1]].visited, 100, 15, FONT_NORMAL, LCD_OPAQUE);
  988. if(map[currentLoc[0]][currentLoc[1]].landmark->wall_north) FA_LCDLine(0,5,5,5);
  989. if(map[currentLoc[0]][currentLoc[1]].landmark->wall_east) FA_LCDLine(5,5,5,10);
  990. if(map[currentLoc[0]][currentLoc[1]].landmark->wall_south) FA_LCDLine(0,10,5,10);
  991. if(map[currentLoc[0]][currentLoc[1]].landmark->wall_west) FA_LCDLine(0,5,0,10);
  992. /*int x = 5;
  993. int y = 5;
  994. FA_LCDLine(x-5,y,x,y);
  995. FA_LCDLine(x,y,x,y+5);
  996. FA_LCDLine(x-5,y+5,x,y+5);
  997. FA_LCDLine(x-5,y,x-5,y+5);
  998.  
  999. FA_LCDLine(x-5+10,y,x+10,y);
  1000. FA_LCDLine(x+10,y,x+10,y+5);
  1001. FA_LCDLine(x-5+10,y+5,x+10,y+5);
  1002. FA_LCDLine(x-5+10,y,x-5+10,y+5);
  1003.  
  1004. FA_LCDLine(x-5+20,y,x+20,y);
  1005. FA_LCDLine(x+20,y,x+20,y+5);
  1006. FA_LCDLine(x-5+20,y+5,x+20,y+5);
  1007. FA_LCDLine(x-5+20,y,x-5+20,y+5);
  1008.  
  1009. FA_LCDLine(x-5+30,y,x+30,y);
  1010. FA_LCDLine(x+30,y,x+30,y+5);
  1011. FA_LCDLine(x-5+30,y+5,x+30,y+5);
  1012. FA_LCDLine(x-5+30,y,x-5+30,y+5);*/
  1013. /*int i, j;
  1014. int y = 5;
  1015. for(i = 0; i < 4; i++) {
  1016. int x = 5;
  1017. for(j = 0; j < 4; i++) {
  1018. if(map[i][j].landmark->wall_north) FA_LCDLine(x-5,y,x,y);
  1019. if(map[i][j].landmark->wall_east) FA_LCDLine(x,y,x,y+5);
  1020. if(map[i][j].landmark->wall_south) FA_LCDLine(x-5,y+5,x,y+5);
  1021. if(map[i][j].landmark->wall_west) FA_LCDLine(x-5,y,x-5,y+5);
  1022. x = x + 10;
  1023. }
  1024. y = y + 10;
  1025. }*/
  1026. }
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!