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