Advertisement
Bosssu

Project 5-6 complete (Sensor)

Jul 22nd, 2014
264
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.08 KB | None | 0 0
  1. #include <atx.h>
  2. #define BLACK 300
  3. #define LEFT_SENSOR 2
  4. #define RIGHT_SENSOR 3
  5. #define LFM 0
  6. #define LBM 1
  7. #define RFM 5
  8. #define RBM 4
  9. void setup()
  10. {
  11. sw1_press();
  12. }
  13.  
  14. void loop()
  15. {
  16. int left = analogRead (LEFT_SENSOR);
  17. int right = analogRead (RIGHT_SENSOR);
  18. if(left < BLACK && right < BLACK)
  19. {
  20. motor_stop(ALL);
  21. }
  22. else if (left < BLACK)
  23. {
  24. turn_left(100,50);
  25. motor_stop(ALL);
  26. }
  27. else if (right < BLACK)
  28. {
  29. turn_right(100,50);
  30. motor_stop(ALL);
  31. }
  32. else
  33. {
  34. go_forward(10,100);
  35. motor_stop(ALL);
  36. }
  37. }
  38. void go_forward(int power,int ms)
  39. {
  40. motor(LFM,power);
  41. motor(LBM,power);
  42. motor(RFM,power);
  43. motor(RBM,power);
  44. delay(ms);
  45. motor_stop(ALL);
  46. }
  47. void turn_right(int power,int ms)
  48. {
  49. motor(LFM,power);
  50. motor(LBM,power);
  51. motor(RFM,-power);
  52. motor(RBM,-power);
  53. delay(ms);
  54. motor_stop(ALL);
  55. }
  56. void turn_left(int power,int ms)
  57. {
  58. motor(LFM,-power);
  59. motor(LBM,-power);
  60. motor(RFM,power);
  61. motor(RBM,power);
  62. delay(ms);
  63. motor_stop(ALL);
  64. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement