| 1 | /* |
| 2 | This program is free software: you can redistribute it and/or modify |
| 3 | it under the terms of the GNU Affero General Public License as |
| 4 | published by the Free Software Foundation, either version 3 of the |
| 5 | License, or (at your option) any later version. |
| 6 | |
| 7 | This program is distributed in the hope that it will be useful, |
| 8 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 10 | GNU Affero General Public License for more details. |
| 11 | |
| 12 | You should have received a copy of the GNU Affero General Public License |
| 13 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 14 | */ |
| 15 | /* |
| 16 | Motor. |
| 17 | Runs both motors back and forth. |
| 18 | n |
| 19 | This example code is in the public domain. |
| 20 | */ |
| 21 | |
| 22 | int motor1_enable = 10; |
| 23 | int motor1a = 9; |
| 24 | int motor1b = 8; |
| 25 | int motor2_enable = 3; |
| 26 | int motor2a = 4; |
| 27 | int motor2b = 5; |
| 28 | |
| 29 | // ping sensors |
| 30 | int left_trigger = 12; |
| 31 | int left_echo = 11; |
| 32 | int right_trigger = 7; |
| 33 | int right_echo = 6; |
| 34 | |
| 35 | // the setup routine runs once when you press reset: |
| 36 | void setup() { |
| 37 | Serial.begin(9600); |
| 38 | |
| 39 | // initialize the digital pin as an output. |
| 40 | pinMode(motor1_enable, OUTPUT); |
| 41 | pinMode(motor1a, OUTPUT); |
| 42 | pinMode(motor1b, OUTPUT); |
| 43 | pinMode(motor2_enable, OUTPUT); |
| 44 | pinMode(motor2a, OUTPUT); |
| 45 | pinMode(motor2b, OUTPUT); |
| 46 | |
| 47 | // ping sensors |
| 48 | pinMode(left_trigger, OUTPUT); |
| 49 | pinMode(left_echo, INPUT); |
| 50 | pinMode(right_trigger, OUTPUT); |
| 51 | pinMode(right_echo, INPUT); |
| 52 | |
| 53 | digitalWrite(motor1_enable, LOW); |
| 54 | digitalWrite(motor1a, LOW); |
| 55 | digitalWrite(motor1b, LOW); |
| 56 | digitalWrite(motor2_enable, LOW); |
| 57 | digitalWrite(motor2a, LOW); |
| 58 | digitalWrite(motor2b, LOW); |
| 59 | } |
| 60 | |
| 61 | void |
| 62 | on(int pin) |
| 63 | { |
| 64 | digitalWrite (pin, HIGH); |
| 65 | } |
| 66 | |
| 67 | void |
| 68 | off(int pin) |
| 69 | { |
| 70 | digitalWrite (pin, LOW); |
| 71 | } |
| 72 | |
| 73 | int |
| 74 | ping(int trigger, int echo) |
| 75 | { |
| 76 | int ping_time; |
| 77 | on(trigger); |
| 78 | delayMicroseconds(12); |
| 79 | off(trigger); |
| 80 | ping_time = pulseIn (echo, HIGH); |
| 81 | if (ping_time <= 0) |
| 82 | ping_time = 3000; |
| 83 | return ping_time; |
| 84 | } |
| 85 | |
| 86 | void motorsRun(int left, int right, int ms_delay) { |
| 87 | // Set left motor direction: |
| 88 | if (left > 0) { |
| 89 | // Set left motor to go forward: |
| 90 | digitalWrite(motor1a, HIGH); |
| 91 | digitalWrite(motor1b, LOW); |
| 92 | } else { |
| 93 | // Set left motor to go backward: |
| 94 | digitalWrite(motor1a, LOW); |
| 95 | digitalWrite(motor1b, HIGH); |
| 96 | left = -left; // Make left a positive value: |
| 97 | } |
| 98 | analogWrite(motor1_enable, left); // Start motor in right direction |
| 99 | |
| 100 | // Set left motor direction: |
| 101 | if (right > 0) { |
| 102 | // Set right motor to go forward: |
| 103 | digitalWrite(motor2a, HIGH); |
| 104 | digitalWrite(motor2b, LOW); |
| 105 | } else { |
| 106 | // Set right motor to go backward: |
| 107 | digitalWrite(motor2a, LOW); |
| 108 | digitalWrite(motor2b, HIGH); |
| 109 | right = -right; // Make right a positive value: |
| 110 | } |
| 111 | analogWrite(motor2_enable, right); // Start motor in right direction |
| 112 | |
| 113 | delay(ms_delay); // Wait the specified amount of time |
| 114 | } |
| 115 | |
| 116 | // the loop routine runs over and over again forever: |
| 117 | void |
| 118 | loop() |
| 119 | { |
| 120 | int left_speed = 0; |
| 121 | int right_speed = 0; |
| 122 | |
| 123 | int left_ping = ping (left_trigger, left_echo); |
| 124 | int right_ping = ping (right_trigger, right_echo); |
| 125 | |
| 126 | /* |
| 127 | Serial.print ("left ping = "); |
| 128 | Serial.print (left_ping); |
| 129 | |
| 130 | Serial.print (" right ping = "); |
| 131 | Serial.println (right_ping); |
| 132 | */ |
| 133 | |
| 134 | if (left_ping < 400) |
| 135 | { |
| 136 | left_speed = 250; |
| 137 | } |
| 138 | |
| 139 | if (right_ping < 400) |
| 140 | { |
| 141 | right_speed = 250; |
| 142 | } |
| 143 | |
| 144 | if (left_speed == 0 && right_speed == 0) |
| 145 | { |
| 146 | // backup |
| 147 | motorsRun(-250, -250, 2000); |
| 148 | // turn around |
| 149 | motorsRun(-250, 250, 3200); |
| 150 | } |
| 151 | else |
| 152 | { |
| 153 | motorsRun(left_speed, right_speed, 0); |
| 154 | } |
| 155 | } |