API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 16th, 2018
78
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 23.70 KB | None | 0 0
raw download clone embed print report
  1. /* ========================================
  2. *
  3. * Copyright YOUR COMPANY, THE YEAR
  4. * All Rights Reserved
  5. * UNPUBLISHED, LICENSED SOFTWARE.
  6. *
  7. * CONFIDENTIAL AND PROPRIETARY INFORMATION
  8. * WHICH IS THE PROPERTY OF your company.
  9. *
  10. * ========================================
  11. */
  12. #include "project.h"
  13. #include "math.h"
  14. #include "stdio.h"
  15. #include "stdlib.h"
  16. #define LEFT (6)
  17. #define RIGHT (9)
  18. #define FORW (5)
  19. #define BACK (10)
  20. //VARIABLE DECLARATION
  21. //photodiode
  22. float brtnes1,brtnes2,brtnes3;
  23. char s[50];
  24. int result1;
  25. int result2;
  26. int result3;
  27. char pattern[3] = {};
  28. char color;
  29. int n =0;
  30. //Ultrasonic
  31. uint16 count = 0;
  32. //RIGHT
  33. int rightFRONTDist;
  34. int rightBACKDist;
  35. int distArrayRF[3] = {};
  36. int distArrayRB[3] = {};
  37. //FRONT
  38. int frontDist;
  39. int distArrayF[3] = {};
  40. //LEFT
  41. int leftFRONTDist;
  42. int leftBACKDist;
  43. int distArrayLF[3] = {};
  44. int distArrayLB[3] = {};
  45. int distIndex = 0;
  46. //Variable declaration for speed and distance travelled
  47. uint8 period = 255; //For PWM
  48. uint8 leftSpeed = 200; // defines speed using PWM
  49. uint8 rightSpeed = 200; // defines speed using PWM
  50. int wheelCirc =14606 - 944; // revolution of wheel at >>DOESNT MATTER WHAT cmp (125)<< x
  51. float C = 3.1415 * 2.5*2; // circumference of wheel in cm
  52. int robotCirc = 25000; // half revolution of robot y
  53. //Shaft encoder counts
  54. int16 LeftCount = 0;
  55. int16 RightCount = 0;
  56. //Used for termite
  57. char String[50];
  58. //Interrupt Handlers
  59. //Ultrasonic Distance measuring interrupt handler
  60. CY_ISR(Timer_ISR_Handler){
  61.  
  62. //Clearing edgecapture register
  63. timer_rightFRONT_ReadStatusRegister();
  64. count = 0;
  65. count = timer_rightFRONT_ReadCounter();
  66. rightFRONTDist = (65535-count)/58;
  67.  
  68. }
  69. CY_ISR(Timer_ISR_Handler2){
  70.  
  71. //Clearing edgecapture register
  72. timer_rightBACK_ReadStatusRegister();
  73. count =0;
  74. count = timer_rightBACK_ReadCounter();
  75. rightBACKDist = (65535-count)/58;
  76. }
  77. CY_ISR(Timer_ISR_Handler3){
  78.  
  79. //Clearing edgecapture register
  80. Timer_FRONTUS_ReadStatusRegister();
  81. count = Timer_FRONTUS_ReadCounter();
  82. frontDist = (65535-count)/58;
  83. }
  84. //LEFT
  85. CY_ISR(Timer_ISR_Handler4){
  86.  
  87. //Clearing edgecapture register
  88. timer_leftFRONT_ReadStatusRegister();
  89. count = 0;
  90. count = timer_leftFRONT_ReadCounter();
  91. leftFRONTDist = (65535-count)/58;
  92.  
  93. }
  94. CY_ISR(Timer_ISR_Handler5){
  95.  
  96. //Clearing edgecapture register
  97. timer_leftBACK_ReadStatusRegister();
  98. count =0;
  99. count = timer_leftBACK_ReadCounter();
  100. leftBACKDist = (65535-count)/58;
  101. }
  102. //Prototypes - INDIVIDUAL FUNCTIONS
  103. void termite();
  104. void turn(char dir);
  105. void move(float dist);
  106. void StopMoving();
  107. void reverse(float dist);
  108. void ultrasonic(char side);
  109. void straighten();
  110. void adjust(char side);
  111. void moveUntil(float dist, char speed);
  112. void up();
  113. void down();
  114. void grip();
  115. void drop();
  116. void colorSense();
  117. void setSpeed(int LeftSpeed, int RightSpeed);
  118. void resetDecoders();
  119. void getDecoders();
  120. void showPattern();
  121. void release();
  122. void swap(int *xp, int *yp);
  123. void bubbleSort(int arr[], int n);
  124. void halfTurn(char direction, float factor);
  125. void sensePattern();
  126. void sideDistToWall(float dist, char INout, char side);
  127. //COMBINED FUNCTIONS
  128. void lift();
  129. void moveAlongWall(int fixedDist);
  130. void moveToPuck(int fixedDist);
  131. void puckAlign();
  132. void goToPuck();
  133. void construct();
  134. void home();
  135. void lostboy();
  136. void checkDist();
  137. void ultrasonicR();
  138. void ultrasonicL();
  139.  
  140.  
  141. int main(void)
  142. {
  143.  
  144. //Start components
  145.  
  146. CyGlobalIntEnable; /* Enable global interrupts. */
  147. IDAC8_1_Start();
  148. TIA_1_Start();
  149. IDAC8_1_Sleep();
  150. ADC_DelSig_1_Start();
  151.  
  152. //Start components
  153. UART_1_Start();
  154. RightWheelDecoder_Start();
  155. LeftWheelDecoder_Start();
  156. USrightFRONTisr_StartEx(Timer_ISR_Handler);
  157. USrightBACKisr_StartEx(Timer_ISR_Handler2);
  158. USfrontisr_StartEx(Timer_ISR_Handler3);
  159. USleftFRONTisr_StartEx(Timer_ISR_Handler4);
  160. USleftBACKisr_StartEx(Timer_ISR_Handler5);
  161. timer_rightFRONT_Start(); //RIGHT
  162. timer_rightBACK_Start();
  163. Timer_FRONTUS_Start();
  164. timer_leftFRONT_Start(); //LEFT
  165. timer_leftBACK_Start();
  166.  
  167. MotorSpeedControl_Start();
  168. Servo_Start();
  169. StopMoving();
  170. resetDecoders();
  171. MotorDirectionControl_Write(0);
  172. LED_Write(1);
  173. //back and forth lift and stack
  174. /* 1,2,3,9,4,4,11,10,
  175. 7,7,11,10,
  176. 6,9,5,5,7,7,11,10,
  177. 2,1,3,
  178. 2,9,4,4,11,10,
  179. 7,7,6*/
  180. //reverse and turning test
  181. //4,4,8,9,5,5
  182. //Move and adjust test
  183. //11,10,7,4,4,11,10
  184. //straight right pick up
  185. // 8,5,11,10,
  186. // 7,1,1,3,2,2
  187. //STRAIGHT RIGHT PICK UP
  188. //8,11,12,5,11,10,7,11,14,15,11,10,
  189. //1,1,3,2,2
  190. int actions[100] = {
  191.  
  192. 12
  193. };
  194. //
  195. int flag = 0;
  196. int i = 0;
  197. release();
  198. StepperEnable1_Write(1);
  199. StepperEnable2_Write(1);
  200. for(;;)
  201. {
  202. sprintf(String, "RF: %d \n",rightFRONTDist);
  203. UART_1_PutString(String);
  204. sprintf(String, "RB: %d \n",rightBACKDist);
  205. UART_1_PutString(String);
  206. sprintf(String, "FRONT: %d \n",frontDist);
  207. CyDelay(500);
  208. UART_1_PutString(String);
  209. sprintf(String, "LF: %d \n",leftFRONTDist);
  210. UART_1_PutString(String);
  211. sprintf(String, "LB: %d \n",leftBACKDist);
  212. UART_1_PutString(String);
  213.  
  214. /* Place your application code here. */
  215. if(SW_Read() == 0 ){
  216. flag = ~flag;
  217.  
  218. }
  219. if(flag){
  220. LED_Write(0);
  221. switch(actions[i]){
  222. case 1:CyDelay(100);
  223. lift();
  224. break;
  225.  
  226. case 2: moveAlongWall(30);
  227. break;
  228.  
  229. case 3:
  230. // spare
  231. break;
  232.  
  233. case 4: puckAlign();
  234. break;
  235.  
  236. case 5: sensePattern();
  237. break;
  238.  
  239. case 6: sideDistToWall(10 , 'O', 'L');
  240. break;
  241.  
  242. case 7: sideDistToWall( 15 , 'I', 'L');
  243. break;
  244.  
  245. case 8: turn('R');
  246. break;
  247.  
  248. case 9:
  249. // spare
  250. break;
  251.  
  252. case 10:
  253. CyDelay(100);
  254. adjust('L');
  255. break;
  256.  
  257. case 11:
  258. CyDelay(100);
  259. ultrasonic('L');
  260. break;
  261.  
  262. case 12:
  263. CyDelay(100);
  264. moveUntil(22,'F');
  265. break;
  266.  
  267. case 13: lostboy();
  268. break;
  269. }
  270.  
  271. i++;
  272. }
  273.  
  274. CyDelay(100);
  275.  
  276. }
  277. }
  278.  
  279. void drop(){
  280. down();
  281. CyDelay(200);
  282. down();
  283. CyDelay(200);
  284. }
  285. // stacks the pucks when pucks grabbed
  286. void construct(){
  287. CyDelay(100);
  288. turn('R');
  289. moveAlongWall(30);
  290. adjust('R');
  291. moveUntil(20,'F');
  292. sideDistToWall( 14.5,'I','R');
  293. moveUntil(15,'S');
  294. adjust('R');
  295. drop();
  296. release();
  297. lift();
  298.  
  299. }
  300. void lostboy() // when robot is in puck selection position
  301. {
  302. //13,11,12,8turn,15move4,11,10adjust,11,7shuffle14.5,
  303. // 11,10adjust,17reverse(7),11,16gotopuck,1lift,9reverese
  304. move(30);
  305. moveUntil(25,'F');
  306. // can add shuffle for high accuracy
  307. CyDelay(100);
  308. turn('R');
  309. CyDelay(100);
  310. adjust('L');
  311. CyDelay(100);
  312. move(4);
  313. CyDelay(100);
  314. adjust('L'); // if we shuffle dont need this
  315. CyDelay(100);
  316. sideDistToWall(14.5,'I','L'); // aligns to puck
  317.  
  318. adjust('L');
  319. reverse(7);
  320. goToPuck();
  321. lift();
  322. reverse(8);
  323.  
  324. }
  325.  
  326. // Lifts Puck
  327. void lift(){
  328. grip();
  329. CyDelay(200);
  330. up();
  331. CyDelay(200);
  332. up();
  333. }
  334.  
  335. void moveAlongWall(int fixedDist){
  336. move(fixedDist);
  337. move(10);
  338.  
  339. CyDelay(100);
  340. moveUntil(21,'F');
  341. }
  342.  
  343. void puckAlign(){
  344. //first puck
  345. CyDelay(100);
  346. turn('R');
  347. CyDelay(100);
  348. ultrasonic('L');
  349. adjust('L');
  350. CyDelay(100);
  351. move(15);
  352. turn('L');
  353. ultrasonic('L');
  354. adjust('L');
  355. }
  356.  
  357. void sensePattern(){
  358. n = 0;
  359. reverse(7);
  360.  
  361. for( n = 0 ; n < 3; n++){
  362. colorSense();
  363. pattern[n] = color;
  364. move(4.5);
  365. CyDelay(600);
  366. }
  367. }
  368.  
  369. void sideDistToWall(float dist, char INout, char side){
  370. int attempt = 0;
  371. ultrasonic(side);
  372. char notSide;
  373.  
  374. if(side == 'L'){
  375. notSide = 'R';
  376. }
  377. else if(side =='R'){
  378. notSide = 'L';
  379. }
  380.  
  381. if(INout == 'O'){
  382.  
  383. CyDelay(200);
  384. halfTurn(side,0.35);
  385. CyDelay(200);
  386. reverse(20);
  387. CyDelay(200);
  388. halfTurn(notSide,0.35);
  389. CyDelay(200);
  390. ultrasonic(side);
  391. adjust(side);
  392. move(5);
  393.  
  394. }else if(INout == 'I'){
  395. while(attempt ==0){
  396.  
  397. while(rightFRONTDist>dist && rightBACKDist>dist){
  398. attempt = 1;
  399. CyDelay(200);
  400. halfTurn(notSide,0.126);
  401. CyDelay(200);
  402. reverse(5);
  403. CyDelay(200);
  404. halfTurn(side,0.126);
  405. CyDelay(200);
  406. ultrasonic(side);
  407. adjust(side);
  408. move(5);
  409. }
  410.  
  411. ultrasonic(side);
  412. }
  413.  
  414.  
  415. }
  416.  
  417. }
  418.  
  419. void goToPuck(){
  420. CyDelay(100);
  421. checkDist();
  422. CyDelay(100);
  423. moveUntil(14,'S');
  424. CyDelay(100);
  425. ultrasonic('L');
  426. checkDist();
  427. drop();
  428. move(3.5);
  429. }
  430. void checkDist(){
  431.  
  432. if(frontDist > 14){
  433. move(0.5);
  434. }else if(frontDist < 14){
  435. reverse(0.5);
  436. }
  437. }
  438.  
  439. //INDIVIDUAL FUNCTIONS
  440. //ARM FUNCTIONS
  441. void grip(){
  442. Servo_WriteCompare(1400);
  443. }
  444. void release(){
  445. Servo_WriteCompare(1225);
  446. }
  447.  
  448. void up(){
  449. int x = 10000;
  450. for( int i = 0 ; i <50/8 ; i++){
  451. A_Write(1);
  452. Abar_Write(0);
  453. Bbar_Write(0);
  454. B_Write(1);
  455. CyDelayUs(x);
  456.  
  457. A_Write(0);
  458. Abar_Write(1);
  459. Bbar_Write(0);
  460. B_Write(1);
  461. CyDelayUs(x);
  462.  
  463. A_Write(0);
  464. Abar_Write(1);
  465. Bbar_Write(1);
  466. B_Write(0);
  467. CyDelayUs(x);
  468.  
  469. A_Write(1);
  470. Abar_Write(0);
  471. Bbar_Write(1);
  472. B_Write(0);
  473. CyDelayUs(x);
  474. }
  475. }
  476. void down(){
  477. int x = 10000;
  478. for( int i = 0 ; i <50/8 ; i++){
  479. A_Write(1);
  480. Abar_Write(0);
  481. Bbar_Write(1);
  482. B_Write(0);
  483. CyDelayUs(x);
  484.  
  485. A_Write(0);
  486. Abar_Write(1);
  487. Bbar_Write(1);
  488. B_Write(0);
  489. CyDelayUs(x);
  490.  
  491. A_Write(0);
  492. Abar_Write(1);
  493. Bbar_Write(0);
  494. B_Write(1);
  495. CyDelayUs(x);
  496.  
  497. A_Write(1);
  498. Abar_Write(0);
  499. Bbar_Write(0);
  500. B_Write(1);
  501. CyDelayUs(x);
  502. }
  503. }
  504.  
  505. //END OF ARM FUNCTIONS
  506. //COLOR SENSING
  507. void Turn_on_RLight()
  508. {
  509. Red_led_Write(1);
  510. Green_led_Write(0);
  511. Blue_led_Write(0);
  512. }
  513. void Turn_on_GLight()
  514. {
  515. Red_led_Write(0);
  516. Green_led_Write(1);
  517. Blue_led_Write(0);
  518. }
  519. void Turn_on_BLight()
  520. {
  521. Red_led_Write(0);
  522. Green_led_Write(0);
  523. Blue_led_Write(1);
  524. }
  525. void Turn_off_Light()
  526. {
  527. Green_led_Write(0);
  528. Blue_led_Write(0);
  529. Red_led_Write(0);
  530. }
  531. int resul_record()
  532. {
  533. int32 resu=0;
  534. ADC_DelSig_1_StartConvert();
  535. ADC_DelSig_1_IsEndConversion(ADC_DelSig_1_WAIT_FOR_RESULT);
  536. resu=ADC_DelSig_1_GetResult32();
  537. return resu;
  538. }
  539. float brightness_Mean_calculation()
  540. {
  541.  
  542. int mean;
  543. int count=0;
  544. int i;
  545. for(i=0;i<20;i++)
  546. {
  547. count+=abs(ADC_DelSig_1_GetResult32());
  548. }
  549. mean=(int) count/20;
  550. return mean;
  551. }
  552. void comp(int brt1,int brt2,int brt3)
  553. {
  554. int i;
  555.  
  556. if( (brt1>brt2) && (brt1>brt3) )
  557. {
  558. for(i=0;i<10;i++)
  559. {
  560. Red_led_Write(1);
  561. CyDelay(100);
  562. Red_led_Write(0);
  563. CyDelay(100);
  564.  
  565. }
  566. color = 'R';
  567.  
  568. }
  569. else if ( (brt2>brt1) && (brt2>brt3) )
  570. {
  571. for(i=0;i<10;i++)
  572. {
  573.  
  574. Green_led_Write(1);
  575. CyDelay(100);
  576. Green_led_Write(0);
  577. CyDelay(100);
  578.  
  579. }
  580. color = 'G';
  581.  
  582.  
  583. }
  584. else if( (brt3>brt1) && (brt3>brt2) )
  585. {
  586. for(i=0;i<10;i++)
  587. {
  588. Blue_led_Write(1);
  589. CyDelay(100);
  590. Blue_led_Write(0);
  591. CyDelay(100);
  592. }
  593. color = 'B';
  594.  
  595. }
  596.  
  597.  
  598. }
  599. void colorSense(){
  600.  
  601.  
  602.  
  603. IDAC8_1_Wakeup();
  604. Turn_on_RLight();
  605. CyDelay(50);
  606. result1 = resul_record();
  607. //brtnes1=brightness_Mean_calculation();
  608. Turn_off_Light();
  609. IDAC8_1_Sleep();
  610. CyDelay(10);
  611.  
  612.  
  613.  
  614. IDAC8_1_Wakeup();
  615. Turn_on_GLight();
  616. CyDelay(50);
  617. result2 = resul_record();
  618. //brtnes2=brightness_Mean_calculation();
  619. Turn_off_Light();
  620. IDAC8_1_Sleep();
  621. CyDelay(10);
  622.  
  623.  
  624. IDAC8_1_Wakeup();
  625. Turn_on_BLight();
  626. CyDelay(50);
  627. result3 = resul_record();
  628. //brtnes3=brightness_Mean_calculation();
  629. Turn_off_Light();
  630. IDAC8_1_Sleep();
  631. CyDelay(500);
  632.  
  633.  
  634. comp(result1,result2,result3);
  635. CyDelay(500);
  636.  
  637. }
  638. void showPattern(){
  639. for(int i = 0; i <3 ; i++){
  640. if(pattern[i] == 'R'){
  641. Red_led_Write(1);
  642. CyDelay(500);
  643. Red_led_Write(0);
  644.  
  645. }else if(pattern[i] == 'G'){
  646. Green_led_Write(1);
  647. CyDelay(500);
  648. Green_led_Write(0);
  649.  
  650. }else if(pattern[i] == 'B'){
  651. Blue_led_Write(1);
  652. CyDelay(500);
  653. Blue_led_Write(0);
  654.  
  655.  
  656. }
  657.  
  658.  
  659.  
  660. CyDelay(500);
  661. }
  662. }
  663. //END OF COLOR SENSING
  664. //MOTOR
  665. //MOVEMENT
  666. void adjust(char side){
  667. int frnt;//distances of side ultrasonics
  668. int bck;
  669.  
  670. int direction;//TURNING DIRECTION
  671. int oppDirection;
  672. ultrasonic('L');
  673. if(side == 'L'){
  674. frnt = leftFRONTDist;
  675. bck = leftBACKDist;
  676. direction = LEFT;
  677. oppDirection = RIGHT;
  678. }else if( side =='R'){
  679. frnt = rightFRONTDist;
  680. bck = rightBACKDist;
  681. direction = RIGHT;
  682. oppDirection = LEFT;
  683. }
  684.  
  685.  
  686. MotorDirectionControl_Write(direction);
  687. while( ((frnt-0.5)-bck) > 0 ){
  688. rightSpeed = 100;
  689. leftSpeed = 0;
  690. setSpeed(leftSpeed,rightSpeed);
  691. CyDelay(50);
  692. StopMoving();
  693. ultrasonic(side);
  694. }
  695. MotorDirectionControl_Write(oppDirection);
  696. while( (rightBACKDist - (rightFRONTDist-0.5))>0 ){
  697.  
  698. rightSpeed = 0;
  699. leftSpeed = 100;
  700. setSpeed(leftSpeed,rightSpeed);
  701. CyDelay(50);
  702. StopMoving();
  703. ultrasonic(side);
  704. }
  705.  
  706. StopMoving();
  707. }
  708. //Functions
  709. //For debuggings purposes
  710. void termite(){
  711. sprintf(String, "sidefront: %d \n",rightFRONTDist);
  712. UART_1_PutString(String);
  713. sprintf(String, "sideback: %d \n",rightBACKDist);
  714. UART_1_PutString(String);
  715. sprintf(String, "FRONT: %d \n",frontDist);
  716. UART_1_PutString(String);
  717. //sprintf(String, "SE2: %d \n",RightCount);
  718. //UART_1_PutString(String);
  719. }
  720. //Ultrasonic
  721. void ultrasonicR(){
  722. CyDelay(50);
  723. int n= 0;
  724. for ( n = 0 ; n <3 ; n ++){
  725. while(ECHO_rightFRONT_Read() == 0){
  726. TRIG_rightFRONT_Write(1);
  727. CyDelayUs(10);
  728. TRIG_rightFRONT_Write(0);
  729.  
  730.  
  731. }
  732.  
  733. distArrayRF[n] = rightFRONTDist;
  734. CyDelay(100);
  735. while(ECHO_rightBACK_Read() == 0){
  736. TRIG_rightBACK_Write(1);
  737. CyDelayUs(10);
  738. TRIG_rightBACK_Write(0);
  739.  
  740.  
  741. }
  742. distArrayRB[n] = rightBACKDist;
  743. CyDelay(100);
  744. while(ECHO_FRONT_Read() == 0){
  745. TRIG_FRONT_Write(1);
  746. CyDelayUs(10);
  747. TRIG_FRONT_Write(0);
  748.  
  749.  
  750. }
  751. distArrayF[n] = frontDist;
  752. CyDelay(100);
  753. }
  754. bubbleSort(distArrayRF, 3);
  755. bubbleSort(distArrayRB, 3);
  756. bubbleSort(distArrayF, 3);
  757. rightFRONTDist = distArrayRF[1];
  758. rightBACKDist = distArrayRB[1];
  759. frontDist = distArrayF[1];
  760. }
  761.  
  762. void ultrasonicL(){
  763. CyDelay(50);
  764. int n= 0;
  765. for ( n = 0 ; n <3 ; n ++){
  766. while(ECHO_leftFRONT_Read() == 0){
  767. TRIG_leftFRONT_Write(1);
  768. CyDelayUs(10);
  769. TRIG_leftFRONT_Write(0);
  770.  
  771.  
  772. }
  773.  
  774. distArrayLF[n] = leftFRONTDist;
  775. CyDelay(100);
  776. while(ECHO_leftBACK_Read() == 0){
  777. TRIG_leftBACK_Write(1);
  778. CyDelayUs(10);
  779. TRIG_leftBACK_Write(0);
  780.  
  781.  
  782. }
  783. distArrayLB[n] = leftBACKDist;
  784. CyDelay(100);
  785. while(ECHO_FRONT_Read() == 0){
  786. TRIG_FRONT_Write(1);
  787. CyDelayUs(10);
  788. TRIG_FRONT_Write(0);
  789.  
  790.  
  791. }
  792. distArrayF[n] = frontDist;
  793. CyDelay(100);
  794. }
  795. bubbleSort(distArrayLF, 3);
  796. bubbleSort(distArrayLB, 3);
  797. bubbleSort(distArrayF, 3);
  798. leftFRONTDist = distArrayLF[1];
  799. leftBACKDist = distArrayLB[1];
  800. frontDist = distArrayF[1];
  801. }
  802. void ultrasonic(char side){
  803. if(side == 'L'){
  804. ultrasonicL();
  805. }
  806. else if(side == 'R'){
  807. ultrasonicR();
  808. }
  809. }
  810.  
  811.  
  812.  
  813. void swap(int *xp, int *yp)
  814. {
  815. int temp = *xp;
  816. *xp = *yp;
  817. *yp = temp;
  818. }
  819.  
  820. // A function to implement bubble sort
  821. void bubbleSort(int arr[], int n)
  822. {
  823. int i, j;
  824. for (i = 0; i < n-1; i++)
  825.  
  826. // Last i elements are already in place
  827. for (j = 0; j < n-i-1; j++)
  828. if (arr[j] > arr[j+1])
  829. swap(&arr[j], &arr[j+1]);
  830. }
  831. void turn(char direction){
  832. resetDecoders();
  833. getDecoders();
  834. switch(direction){
  835. case 'L':
  836. MotorDirectionControl_Write(LEFT);
  837. while(abs(RightCount)<13900 && abs(LeftCount)<13900){setSpeed(170,170);getDecoders();}
  838. StopMoving();
  839. break;
  840. case 'R':
  841. MotorDirectionControl_Write(RIGHT);
  842. while(abs(RightCount)<13900 && abs(LeftCount)<13900){setSpeed(170,170);getDecoders();}
  843. StopMoving();
  844. break;
  845. }
  846. resetDecoders();
  847. }
  848. void halfTurn(char direction, float factor){
  849.  
  850. resetDecoders();
  851. getDecoders();
  852. switch(direction){
  853. case 'L':
  854. MotorDirectionControl_Write(LEFT);
  855. while(abs(RightCount)<13900*factor && abs(LeftCount)<13900*factor)
  856. {setSpeed(170,170);getDecoders();}
  857. StopMoving();
  858. break;
  859. case 'R':
  860. MotorDirectionControl_Write(RIGHT);
  861. while(abs(RightCount)<13900*factor && abs(LeftCount)<13900*factor)
  862. {setSpeed(170,170);getDecoders();}
  863. StopMoving();
  864. break;
  865. }
  866. resetDecoders();
  867. }
  868. //Move forward dist cm
  869. void move(float dist){
  870. //Number of cirumferences to move a distance of dist
  871. float factor = dist/C;
  872. int targetCount = factor* wheelCirc;
  873. int rightSpeed = 140;
  874. int leftSpeed = 140;
  875. int diff = 0;
  876. int switchToRIGHT = 1;
  877. int switchToLEFT = 1;
  878.  
  879. resetDecoders();
  880. getDecoders();
  881. MotorDirectionControl_Write(FORW);
  882. setSpeed(leftSpeed,rightSpeed);
  883. //Forward
  884. while(LeftCount<targetCount && RightCount < targetCount ){
  885. setSpeed(leftSpeed,rightSpeed);
  886. diff = RightCount - LeftCount;
  887. if(diff > 0){ //deviating left //turn right
  888. if(switchToRIGHT){
  889. leftSpeed = rightSpeed +1;
  890. switchToRIGHT = 0;
  891. switchToLEFT = 1;
  892. }else if(!switchToRIGHT){
  893. leftSpeed ++;
  894. }
  895.  
  896. }else if(diff < 0){ //deviating right //turn left
  897. if(switchToLEFT){
  898. leftSpeed = rightSpeed;
  899. switchToRIGHT = 1;
  900. switchToLEFT = 0;
  901. }else if(!switchToLEFT){
  902. leftSpeed -= 1;
  903. }
  904.  
  905. }
  906.  
  907. getDecoders();
  908.  
  909. CyDelay(50);
  910. }
  911.  
  912. StopMoving();
  913.  
  914. }//END OF FIXED MOVEMENT FORWARD
  915. void moveUntil(float dist, char speed){
  916. int rightSpeed = 100;
  917. int leftSpeed = 100;
  918. int del;
  919. int diff = 0;
  920. int switchToRIGHT = 1;
  921. int switchToLEFT = 1;
  922. ultrasonic('L');
  923. if(speed =='F'){
  924. del = 200;
  925. }else{
  926. del = 50;
  927. }
  928. MotorDirectionControl_Write(FORW);
  929. resetDecoders();
  930. getDecoders();
  931. ultrasonic('L');
  932. setSpeed(leftSpeed,rightSpeed);
  933. while(frontDist>(dist)){
  934. setSpeed(leftSpeed,rightSpeed);
  935.  
  936. diff = RightCount - LeftCount;
  937.  
  938.  
  939.  
  940.  
  941. if(diff > 0){ //deviating left //turn right
  942. if(switchToRIGHT){
  943. leftSpeed = rightSpeed +1;
  944. switchToRIGHT = 0;
  945. switchToLEFT = 1;
  946. }else if(!switchToRIGHT){
  947. leftSpeed ++;
  948. }
  949.  
  950. }else if(diff < 0){ //deviating right //turn left
  951. if(switchToLEFT){
  952. leftSpeed = rightSpeed;
  953. switchToRIGHT = 1;
  954. switchToLEFT = 0;
  955. }else if(!switchToLEFT){
  956. leftSpeed -= 1;
  957. }
  958.  
  959. }
  960.  
  961. getDecoders();
  962.  
  963. CyDelay(del);
  964. StopMoving();
  965. ultrasonic('L');
  966. // sprintf(String, "dist; %d \n", frontDist);
  967. //UART_1_PutString(String);
  968. }
  969. StopMoving();
  970. //sprintf(String, "dist; %d \n", frontDist);
  971. // UART_1_PutString(String);
  972.  
  973. }//END OF MOVEMENT UNTIL DISTANCE SENSED
  974. //REVERSE
  975.  
  976. void reverse(float dist){
  977. float factor = dist/C;
  978. int targetCount = factor* wheelCirc;
  979. int rightSpeed = 170;
  980. int leftSpeed = 170;
  981. int switchToRIGHT = 1;
  982. int switchToLEFT = 1;
  983. int diff = 0;
  984.  
  985. resetDecoders();
  986. getDecoders();
  987. setSpeed(leftSpeed,rightSpeed);
  988. MotorDirectionControl_Write(BACK);
  989. while(abs(LeftCount)<targetCount && abs(RightCount) <targetCount ){
  990. setSpeed(leftSpeed,rightSpeed);
  991. diff = RightCount - LeftCount;
  992. if(diff < 0){ //deviating left //turn right
  993. if(switchToRIGHT){
  994. leftSpeed = rightSpeed +1;
  995. switchToRIGHT = 0;
  996. switchToLEFT = 1;
  997. }else if(!switchToRIGHT){
  998. leftSpeed ++;
  999. }
  1000.  
  1001. }else if(diff > 0){ //deviating right //turn left
  1002. if(switchToLEFT){
  1003. leftSpeed = rightSpeed;
  1004. switchToRIGHT = 1;
  1005. switchToLEFT = 0;
  1006. }else if(!switchToLEFT){
  1007. leftSpeed -= 1;
  1008. }
  1009.  
  1010. }
  1011.  
  1012. getDecoders();
  1013.  
  1014.  
  1015.  
  1016. CyDelay(100);
  1017. }
  1018. StopMoving();
  1019. resetDecoders();
  1020. }//END OF REVERSE
  1021. //Stop moving
  1022. void StopMoving(){
  1023. MotorSpeedControl_WriteCompare1(0);
  1024. MotorSpeedControl_WriteCompare2(0);
  1025. }
  1026. void resetDecoders(){
  1027. LeftWheelDecoder_SetCounter(0);
  1028. RightWheelDecoder_SetCounter(0);
  1029. }
  1030. void getDecoders(){
  1031. LeftCount = LeftWheelDecoder_GetCounter();
  1032. RightCount = RightWheelDecoder_GetCounter();
  1033. }
  1034. void setSpeed(int LeftSpeed,int RightSpeed){
  1035. MotorSpeedControl_WriteCompare1(LeftSpeed);
  1036. MotorSpeedControl_WriteCompare2(RightSpeed);
  1037. }
  1038. /* [] END OF FILE */
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!