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