From ColdSphinX, 3 Years ago, written in C++.
This paste is a reply to Re: Re: Re: Re: Arduino Nano L293D from Tiny Earthworm - view diff
Embed
  1. #define DEBUG 0
  2.  
  3. //L293D
  4. //Motor A (left)
  5. #define MOTOR_PIN1 9 // Pin 2 of L293
  6. #define MOTOR_PIN2 10 // Pin 7 of L293
  7. //const int MOTOR_PIN1  = 9;  // Pin 2 of L293
  8. //const int MOTOR_PIN2  = 10;  // Pin 7 of L293
  9. //Motor B (right)
  10. #define MOTOR_PIN3 11 // Pin  10 of L293
  11. #define MOTOR_PIN4 12 // Pin  15 of L293
  12. //const int MOTOR_PIN3  = 11; // Pin  10 of L293
  13. //const int MOTOR_PIN4  = 12;  // Pin  15 of L293
  14.  
  15. //IR Remote
  16. #include <IRremote.h>
  17. #define IR_PIN 8
  18. IRrecv irrecv(IR_PIN);
  19. decode_results ir_results;
  20.  
  21. //Ultrasonic sensor
  22. #define US_TRIG_PIN 7
  23. #define US_ECHO_PIN 6
  24. long duration;
  25. int distance;
  26.  
  27.  
  28.     /*
  29.      * 0xFF38C7 => 5 (stop)
  30.      * 0xFF18E7 => 2 (forward)
  31.      * 0xFF4AB5 => 8 (backward)
  32.      * 0xFF5AA5 => 6 (right rotate)
  33.      * 0xFF10EF => 4 (left rotate)
  34.      * 0xFF30CF => 1 (left circle)
  35.      * 0xFF7A85 => 3 (right circle)
  36.      */
  37. enum movements{
  38.   UNDEFINED,
  39.   STOP,
  40.   FORWARD,
  41.   BACKWARD,
  42.   LEFT_ROTATE,
  43.   RIGHT_ROTATE,
  44.   LEFT_CIRCLE,
  45.   RIGHT_CIRCLE
  46. };
  47.  
  48. enum movements current_mode;
  49. enum movements next_mode;
  50.  
  51. #define TIME_WAIT 3600
  52.  
  53. void setup() {
  54.   randomSeed(analogRead(0));
  55.   //Set pins as outputs
  56.   pinMode(MOTOR_PIN1, OUTPUT);
  57.   pinMode(MOTOR_PIN2, OUTPUT);
  58.   pinMode(MOTOR_PIN3, OUTPUT);
  59.   pinMode(MOTOR_PIN4, OUTPUT);
  60.   stop();
  61.   current_mode = STOP;
  62.   next_mode = STOP;
  63.   pinMode(US_TRIG_PIN, OUTPUT);
  64.   digitalWrite(US_TRIG_PIN, LOW);
  65.   pinMode(US_ECHO_PIN, INPUT);
  66. #if DEBUG > 0
  67.   Serial.begin(9600);
  68.   Serial.println("IR Receiver Raw Data + Button Decode Test");
  69. #endif
  70.   irrecv.enableIRIn(); // Start the receiver
  71. }
  72.  
  73. void motor_a_stop() {
  74.   digitalWrite(MOTOR_PIN1, LOW);
  75.   digitalWrite(MOTOR_PIN2, LOW);
  76. }
  77.  
  78. void motor_a_forward() {
  79.   digitalWrite(MOTOR_PIN1, LOW);
  80.   digitalWrite(MOTOR_PIN2, HIGH);
  81. }
  82.  
  83. void motor_a_backward() {
  84.   digitalWrite(MOTOR_PIN1, HIGH);
  85.   digitalWrite(MOTOR_PIN2, LOW);
  86. }
  87.  
  88. void motor_b_stop() {
  89.   digitalWrite(MOTOR_PIN3, LOW);
  90.   digitalWrite(MOTOR_PIN4, LOW);
  91. }
  92.  
  93. void motor_b_forward() {
  94.   digitalWrite(MOTOR_PIN3, LOW);
  95.   digitalWrite(MOTOR_PIN4, HIGH);
  96. }
  97.  
  98. void motor_b_backward() {
  99.   digitalWrite(MOTOR_PIN3, HIGH);
  100.   digitalWrite(MOTOR_PIN4, LOW);
  101. }
  102.  
  103. void left_stop() {
  104.   motor_a_stop();
  105. }
  106.  
  107. void right_stop() {
  108.   motor_b_stop();
  109. }
  110.  
  111. void stop() {
  112.   left_stop();
  113.   right_stop();
  114. }
  115.  
  116. void left_forward() {
  117.   motor_a_backward();
  118. }
  119.  
  120. void left_backward() {
  121.   motor_a_forward();
  122. }
  123.  
  124. void right_forward() {
  125.   motor_b_forward();
  126. }
  127.  
  128. void right_backward() {
  129.   motor_b_backward();
  130. }
  131.  
  132. void forward() {
  133.   left_forward();
  134.   right_forward();
  135. }
  136.  
  137. void backward() {
  138.   left_backward();
  139.   right_backward();
  140. }
  141.  
  142. void left_rotate() {
  143.   left_backward();
  144.   right_forward();
  145. }
  146.  
  147. void right_rotate() {
  148.   left_forward();
  149.   right_backward();
  150. }
  151.  
  152. void left_circle() {
  153.   left_stop();
  154.   right_forward();
  155. }
  156.  
  157. void right_circle() {
  158.   right_stop();
  159.   left_forward();
  160. }
  161.  
  162. void wait() {
  163.   delay(TIME_WAIT);
  164.   stop();
  165. }
  166.  
  167. void test_movements(){
  168.   forward();
  169.   wait();
  170.   backward();
  171.   wait();
  172.   left_rotate();
  173.   wait();
  174.   right_rotate();
  175.   wait();
  176.   left_circle();
  177.   wait();
  178.   right_circle();
  179.   wait();
  180.   stop();
  181.   wait();
  182. }
  183.  
  184. void ir_decode(){
  185.   switch(ir_results.value){
  186.     /*
  187.      * 0xFF38C7 => 5 (stop)         => STOP
  188.      * 0xFF18E7 => 2 (forward)      => FORWARD
  189.      * 0xFF4AB5 => 8 (backward)     => BACKWARD
  190.      * 0xFF10EF => 4 (left rotate)  => LEFT_ROTATE
  191.      * 0xFF5AA5 => 6 (right rotate) => RIGHT_ROTATE
  192.      * 0xFF7A85 => 3 (right circle) => LEFT_CIRCLE
  193.      * 0xFF30CF => 1 (left circle)  => RIGHT_CIRCLE
  194.      */
  195.     case 0xFF38C7:
  196. #if DEBUG > 0
  197.       Serial.println("Stop (5)");
  198. #endif
  199.       next_mode = STOP;
  200.       break;
  201.  
  202.     case 0xFF18E7:
  203. #if DEBUG > 0
  204.       Serial.println("Forward (2)");
  205. #endif
  206.       next_mode = FORWARD;
  207.       break;
  208.  
  209.     case 0xFF4AB5:
  210. #if DEBUG > 0
  211.       Serial.println("Backward (8)");
  212. #endif
  213.       next_mode = BACKWARD;
  214.       break;
  215.  
  216.     case 0xFF10EF:
  217. #if DEBUG > 0
  218.       Serial.println("Left rotate (4)");
  219. #endif
  220.       next_mode = LEFT_ROTATE;
  221.       break;
  222.  
  223.     case 0xFF5AA5:
  224. #if DEBUG > 0
  225.       Serial.println("Right rotate (6)");
  226. #endif
  227.       next_mode = RIGHT_ROTATE;
  228.       break;
  229.  
  230.     case 0xFF7A85:
  231. #if DEBUG > 0
  232.       Serial.println("Left circle (3)");
  233. #endif
  234.       next_mode = LEFT_CIRCLE;
  235.       break;
  236.  
  237.     case 0xFF30CF:
  238. #if DEBUG > 0
  239.       Serial.println("Right circle (1)");
  240. #endif
  241.       next_mode = RIGHT_CIRCLE;
  242.       break;
  243.  
  244. #if DEBUG > 0
  245.     default:
  246.       Serial.print("Unknown button: ");
  247.       Serial.println(ir_results.value, HEX);
  248. #endif
  249.   }
  250. }
  251.  
  252. void sonic_check(){
  253.   //reset
  254.   digitalWrite(US_TRIG_PIN, LOW);
  255.   delayMicroseconds(2);
  256.   //Set the trigger pin on HIGH state for 10 micro seconds
  257.   digitalWrite(US_TRIG_PIN, HIGH);
  258.   delayMicroseconds(10);
  259.   digitalWrite(US_TRIG_PIN, LOW);
  260.   //Read echo
  261.   duration = pulseIn(US_ECHO_PIN, HIGH);
  262.   //Calculating distance
  263.   distance = duration * 0.034 / 2;
  264. #if DEBUG > 1
  265.   Serial.print("Distance: ");
  266.   Serial.println(distance);
  267. #endif
  268.   if (distance <= 5){
  269.     enum movements last_mode = current_mode;
  270.     stop();
  271.     current_mode = STOP;
  272.     long t = random(100,1000);
  273.     long c = random(10);
  274.     if (c >=5){
  275.         left_rotate();
  276.         current_mode = LEFT_ROTATE;
  277.     } else {
  278.         right_rotate();
  279.         current_mode = RIGHT_ROTATE;
  280.     }
  281.     delay(t);
  282.     next_mode = last_mode;
  283.   }
  284. }
  285.  
  286. void mode_check(){
  287. #if DEBUG > 2
  288.   Serial.println("Mode: ");
  289.   Serial.print("# Current: ");
  290.   Serial.println(current_mode);
  291.   Serial.print("# Next: ");
  292.   Serial.println(next_mode);
  293. #endif
  294.   if (next_mode != current_mode) {
  295.     switch(next_mode){
  296.     /*
  297.      * 0xFF38C7 => 5 (stop)         => STOP
  298.      * 0xFF18E7 => 2 (forward)      => FORWARD
  299.      * 0xFF4AB5 => 8 (backward)     => BACKWARD
  300.      * 0xFF10EF => 4 (left rotate)  => LEFT_ROTATE
  301.      * 0xFF5AA5 => 6 (right rotate) => RIGHT_ROTATE
  302.      * 0xFF7A85 => 3 (right circle) => LEFT_CIRCLE
  303.      * 0xFF30CF => 1 (left circle)  => RIGHT_CIRCLE
  304.      */
  305.       case STOP:
  306.         stop();
  307.         current_mode = STOP;
  308.         break;
  309.       case FORWARD:
  310.         forward();
  311.         current_mode = FORWARD;
  312.         break;
  313.       case BACKWARD:
  314.         backward();
  315.         current_mode = BACKWARD;
  316.         break;
  317.       case LEFT_ROTATE:
  318.         left_rotate();
  319.         current_mode = LEFT_ROTATE;
  320.         break;
  321.       case RIGHT_ROTATE:
  322.         right_rotate();
  323.         current_mode = RIGHT_ROTATE;
  324.         break;
  325.       case LEFT_CIRCLE:
  326.         left_circle();
  327.         current_mode = LEFT_CIRCLE;
  328.         break;
  329.       case RIGHT_CIRCLE:
  330.         right_circle();
  331.         current_mode = RIGHT_CIRCLE;
  332.         break;
  333.     }
  334.   } else {
  335.     delay(500);
  336.   }
  337. }
  338.  
  339. void loop() {
  340.   if (irrecv.decode(&ir_results)){
  341.     ir_decode();
  342.     irrecv.resume(); // receive the next value
  343.   }
  344.   sonic_check();
  345.   mode_check();
  346. #ifdef DEBUG
  347.     //test_movements()
  348. #endif
  349. }