challenge-bot.com Git - challenge-bot/blob - arduino-sketches/follow/follow.ino

   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 // define which pins are connected to which components
  17 int right_motor_speed_pin = 3; // orange wire
  18 int right_motor_forward_pin = 4; // brown
  19 int right_motor_backward_pin = 5; // purple
  20
  21 int right_echo_pin = 6; // white
  22 int right_trigger_pin = 7; // blue
  23
  24 int left_motor_speed_pin = 8; // orange
  25 int left_motor_forward_pin = 9; // brown
  26 int left_motor_backward_pin = 10; // purple
  27
  28 int left_echo_pin = 11; // white
  29 int left_trigger_pin = 12; // blue
  30
  31 // you'll need to adjust these based on your sonar sensor's behavior
  32 int desired_right_ping_time = 800;
  33 int desired_left_ping_time = 800;
  34
  35 void on(int pin){
  36   digitalWrite(pin, HIGH);}
  37
  38 void off(int pin){
  39   digitalWrite(pin, LOW);}
  40
  41 void set_motor(int speed_pin,
  42                int forward_pin,
  43                int backward_pin,
  44                int speed){
  45   if(speed > 0){
  46     off(backward_pin);
  47     on(forward_pin);}
  48   else if(speed < 0){
  49     off(forward_pin);
  50     on(backward_pin);
  51     speed = -speed;}
  52   else{ // speed is 0
  53     off(forward_pin);
  54     off(backward_pin);}
  55   // since speed has been set positive, no need to check if speed < -255.
  56   if(speed > 255){
  57     speed = 255;}
  58   analogWrite(speed_pin, speed);}
  59
  60 void go(int left_motor_speed, int right_motor_speed){
  61   set_motor(left_motor_speed_pin,
  62             left_motor_forward_pin,
  63             left_motor_backward_pin,
  64             left_motor_speed);
  65   set_motor(right_motor_speed_pin,
  66             right_motor_forward_pin,
  67             right_motor_backward_pin,
  68             right_motor_speed);}
  69
  70 int ping(int trigger, int echo){
  71   int ping_time = 0;
  72   // turn off trigger
  73   off(trigger);
  74   delayMicroseconds(2);
  75   // turn on the trigger and leave it on long enough for the
  76   // sonar sensor to notice
  77   on(trigger);
  78   delayMicroseconds(10);
  79   off(trigger);
  80   ping_time = pulseIn(echo, HIGH);
  81   if(ping_time <= 0){
  82     ping_time = 3000;}
  83   // sonar needs some time to recover before pinging again,
  84   // so make sure it gets enough sleep right here.  50 milliseconds
  85   delay(50);
  86   return ping_time;}
  87
  88 void backup(int backup_time){
  89   go(-250, -250);
  90   delay(backup_time);}
  91
  92 void turn_around(int turn_around_time){
  93   go(-250, 250);
  94   delay(turn_around_time);}
  95
  96 void setup(){
  97   Serial.begin(9600);
  98
  99   pinMode(right_motor_speed_pin, OUTPUT);
 100   pinMode(right_motor_forward_pin, OUTPUT);
 101   pinMode(right_motor_backward_pin, OUTPUT);
 102
 103   pinMode(right_echo_pin, INPUT);
 104   pinMode(right_trigger_pin, OUTPUT);
 105
 106   pinMode(left_motor_speed_pin, OUTPUT);
 107   pinMode(left_motor_forward_pin, OUTPUT);
 108   pinMode(left_motor_backward_pin, OUTPUT);
 109
 110   pinMode(left_echo_pin, INPUT);
 111   pinMode(left_trigger_pin, OUTPUT);
 112
 113   off(left_motor_speed_pin);
 114   off(left_motor_forward_pin);
 115   off(left_motor_backward_pin);
 116   off(left_trigger_pin);
 117
 118   off(right_motor_speed_pin);
 119   off(right_motor_forward_pin);
 120   off(right_motor_backward_pin);
 121   off(right_trigger_pin);}
 122
 123 void loop(){
 124   int left_speed;
 125   int right_speed;
 126
 127   int forward_speed = 250;
 128   int stop_speed = 0;
 129
 130   unsigned int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 131   unsigned int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 132
 133   left_speed = actual_left_ping_time - desired_left_ping_time;
 134   right_speed = actual_right_ping_time - desired_right_ping_time;
 135
 136   Serial.print("left: ping = ");
 137   Serial.print(actual_left_ping_time);
 138   Serial.print(" speed = ");
 139   Serial.print(left_speed);
 140   Serial.print(" right: ping = ");
 141   Serial.print(actual_right_ping_time);
 142   Serial.print(" speed = ");
 143   Serial.println(right_speed);
 144
 145   go(left_speed, right_speed);}