challenge-bot.com Git - challenge-bot/blob - build-stages/h_stay_on_table/h_stay_on_table.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 #include "bot_map.h"
  17
  18 void on(int pin){
  19   digitalWrite(pin, HIGH);}
  20
  21 void off(int pin){
  22   digitalWrite(pin, LOW);}
  23
  24 void set_motor(int speed_pin,
  25                int forward_pin,
  26                int backward_pin,
  27                int speed){
  28   if(speed > 0){
  29     off(backward_pin);
  30     on(forward_pin);}
  31   else if(speed < 0){
  32     off(forward_pin);
  33     on(backward_pin);
  34     speed = -speed;}
  35   else{ // speed is 0
  36     off(forward_pin);
  37     off(backward_pin);}
  38   // since speed has been set positive, no need to check if speed < -255.
  39   if(speed > 255){
  40     speed = 255;}
  41   analogWrite(speed_pin, speed);}
  42
  43 void go(int left_motor_speed, int right_motor_speed){
  44   set_motor(left_motor_speed_pin,
  45             left_motor_forward_pin,
  46             left_motor_backward_pin,
  47             left_motor_speed);
  48   set_motor(right_motor_speed_pin,
  49             right_motor_forward_pin,
  50             right_motor_backward_pin,
  51             right_motor_speed);}
  52
  53 int ping(int trigger, int echo){
  54   int ping_time = 0;
  55   // turn off trigger
  56   off(trigger);
  57   delayMicroseconds(2);
  58   // turn on the trigger and leave it on long enough for the
  59   // sonar sensor to notice
  60   on(trigger);
  61   delayMicroseconds(10);
  62   off(trigger);
  63   ping_time = pulseIn(echo, HIGH);
  64   if(ping_time <= 0){
  65     ping_time = 3000;}
  66   // sonar needs some time to recover before pinging again,
  67   // so make sure it gets enough sleep right here.  50 milliseconds
  68   delay(50);
  69   return ping_time;}
  70
  71 void backup(int backup_time){
  72   go(-250, -250);
  73   delay(backup_time);}
  74
  75 void turn_around(int turn_around_time){
  76   go(-250, 250);
  77   delay(turn_around_time);}
  78
  79 void setup(){
  80   Serial.begin(9600);
  81
  82   pinMode(right_motor_speed_pin, OUTPUT);
  83   pinMode(right_motor_forward_pin, OUTPUT);
  84   pinMode(right_motor_backward_pin, OUTPUT);
  85
  86   pinMode(right_echo_pin, INPUT);
  87   pinMode(right_trigger_pin, OUTPUT);
  88
  89   pinMode(left_motor_speed_pin, OUTPUT);
  90   pinMode(left_motor_forward_pin, OUTPUT);
  91   pinMode(left_motor_backward_pin, OUTPUT);
  92
  93   pinMode(left_echo_pin, INPUT);
  94   pinMode(left_trigger_pin, OUTPUT);
  95
  96   off(left_motor_speed_pin);
  97   off(left_motor_forward_pin);
  98   off(left_motor_backward_pin);
  99   off(left_trigger_pin);
 100
 101   off(right_motor_speed_pin);
 102   off(right_motor_forward_pin);
 103   off(right_motor_backward_pin);
 104   off(right_trigger_pin);}
 105
 106 void loop(){
 107   int left_speed;
 108   int right_speed;
 109
 110   int forward_speed = 250;
 111   int stop_speed = 0;
 112
 113   // adjust this number as necessary for your robot.
 114   // it represents how far the table is from your sonar sensor.
 115   // larger values mean larger distance.  default is 800
 116   int right_max_ping_time_over_table = 800;
 117   int left_max_ping_time_over_table = 800;
 118   int backup_time = 1000;
 119   // the exact amount of time for turning around might need
 120   // twerking for your robot.  the default value is 3200
 121   int turn_around_time = 3200;
 122
 123   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 124   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 125
 126   /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
 127   /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
 128
 129   /* Serial.print("left led value = "); */
 130   /* Serial.print(left_led_value); */
 131   /* Serial.print(", right led value = "); */
 132   /* Serial.print(right_led_value); */
 133
 134   /* analogWrite(left_led_pin, left_led_value); */
 135   /* analogWrite(right_led_pin, right_led_value); */
 136
 137   Serial.print("left ping = ");
 138   Serial.print(actual_left_ping_time);
 139   Serial.print("\tright_ping = ");
 140   Serial.println(actual_right_ping_time);
 141
 142   // if the left sonar senses a table, keep driving left side forward,
 143   // otherwise, stop left side
 144   if(actual_left_ping_time < left_max_ping_time_over_table){
 145     left_speed = forward_speed;}
 146   else{
 147     left_speed = stop_speed;}
 148   // if the right sonar senses a table, keep driving right side forward,
 149   // otherwise, stop right side
 150   if(actual_right_ping_time < right_max_ping_time_over_table){
 151     right_speed = forward_speed;}
 152   else{
 153     right_speed = stop_speed;}
 154
 155   // if both sonars detect being off the table, backup and turn around
 156   // otherwise, go the correct speed for each wheel
 157   if(actual_left_ping_time >= left_max_ping_time_over_table
 158      && actual_right_ping_time >= right_max_ping_time_over_table){
 159     Serial.println("backing up");
 160     backup(backup_time);
 161     Serial.println("turning around");
 162     turn_around(turn_around_time);}
 163   else{
 164     Serial.println("going");
 165     go(left_speed, right_speed);}}