Advertisement
Guest User

Untitled

a guest
May 25th, 2016
52
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.30 KB | None | 0 0
  1. #include "simpletools.h" // Include simpletools header
  2. #include "ping.h" // Include ping header
  3. #include "abdrive.h" // Include abdrive header
  4. #define SIDE 40
  5. #define NORTH 0
  6. #define EAST 1
  7. #define SOUTH 2
  8. #define WEST 3
  9. #define cmToTicks (1 / 0.325)
  10. #define STOP 950
  11. #define TURN 1000
  12. #define speed 50
  13. #define factor 2
  14.  
  15.  
  16. int currentDirection = NORTH;
  17.  
  18. void turnDirection(int direction){
  19. drive_ramp(30, 30);
  20. int diff = currentDirection - direction;
  21. drive_ramp(-(30 * diff), (30 * diff));
  22. pause(TURN * abs(diff));
  23. currentDirection = direction;
  24. drive_ramp(128, 128);
  25. return;
  26. }
  27. int checkNext(int input[], int index, int size){
  28. if (index >= size - 1){
  29. return -1;
  30. }
  31. int curr = input[index];
  32. int target = input[index + 1];
  33. int diff = target - curr;
  34. //print("%d\n", diff);
  35. return diff;
  36. }
  37. void move(int input[], int index, int sizeOfInput){
  38. int diff = checkNext(input, index, sizeOfInput);
  39. if(diff == -1){
  40. drive_ramp(0, 0);
  41. return;
  42. }
  43. int counter = 0;
  44. switch (diff){
  45. //move right until diff is 1
  46. case 1:
  47. turnDirection(EAST);
  48. counter = 0;
  49. while(diff == 1){
  50. ++index;
  51. counter++;
  52. diff = checkNext(input, index, sizeOfInput);
  53. }
  54. drive_ramp(128, 128);
  55. pause(counter * STOP);
  56. break;
  57. //move left until diff is -1
  58. case -1:
  59. turnDirection(WEST);
  60. counter = 0;
  61. while(diff == -1){
  62. ++index;
  63. counter++;
  64. diff = checkNext(input, index, sizeOfInput);
  65. }
  66. drive_ramp(128, 128);
  67. pause(counter * STOP);
  68. break;
  69. //move forward until diff is 4
  70. case 4:
  71. turnDirection(NORTH);
  72. counter = 0;
  73. while(diff == 4){
  74. ++index;
  75. counter++;
  76. diff = checkNext(input, index, sizeOfInput);
  77. }
  78. drive_ramp(128, 128);
  79. pause(counter * STOP);
  80. break;
  81. //move backwards until diff is -4
  82. default:
  83. turnDirection(SOUTH);
  84. counter = 0;
  85. while(diff == -4){
  86. ++index;
  87. counter++;
  88. diff = checkNext(input, index, sizeOfInput);
  89. }
  90. drive_ramp(128, 128);
  91. pause(counter * STOP);
  92. }
  93. move(input, index, sizeOfInput);
  94. }
  95.  
  96. int main(){
  97. int inp[] = {-3, 1, 5, 6, 7, 11, 15, 16};
  98. int len = sizeof(inp)/sizeof(inp[0]);
  99. drive_ramp(128, 128);
  100. move(inp, 0, len);
  101. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement