fork download
copy 
  1. #include <Adafruit_CircuitPlayground.h>
  2.  
  3. /* B3_09_Servo_SonicV2.ino
  4.  
  5.    Drive the TWO-WHEELED Bread Board Bot (BBbot, B^3)
  6.    forward.   When within 10 inches of something, turn servos to find clearest path
  7.    and then go that way.
  8.    When a whisker bump sensor on either side hits something,
  9.    back up and turn slightly away from that direction and resume
  10.    forward path.
  11.  
  12.    Stop at a specified distance from an object directly ahead.
  13.  
  14.    Arduino: Arduino Mega 256 v3 Clone
  15.    Motor Shield: Adafruit assembled Motor Shield for Arduino v2
  16.    ---->  http://w...content-available-to-author-only...t.com/products/1438
  17.  
  18.    Programmer: Dave Eslinger; June 13, 2015
  19.    Revisions:   Added sonic sensors on servos, June 23, 2016.
  20. */
  21. #include <Wire.h>
  22. #include <Adafruit_MotorShield.h>
  23. #include <math.h>
  24. #include <BreadBoardBot.h>
  25. #include <NewPing.h>
  26. #include <Servo.h>
  27.  
  28. // Create the motor shield object with the default I2C address
  29. Adafruit_MotorShield AFMS = Adafruit_MotorShield();
  30.  
  31. // Define Servos
  32. Servo panServo;
  33. Servo tiltServo;
  34.  
  35. /* Define Constants */
  36.  
  37. // IO Pins used
  38. const byte LEFT_BUMP_PIN = 47;    // Define DIGITAL Pins for left
  39. const byte RIGHT_BUMP_PIN = 46;   // and right bump sensors
  40. const byte SONIC_TRIGGER_PIN = 51;
  41. const byte SONIC_ECHO_PIN = 50;
  42.  
  43. // Parameters controlling program behavior
  44. // Bump behavior
  45. const byte FORWARD_SPEED = 100;   // Define normal speeds
  46. const byte BACKWARD_SPEED = 60;  // and backup/turn speed
  47. const int  TURN_DURATION = 600;   // Turn length in milliseconds
  48.  
  49. // Sonic sensor
  50. const float TARGET_DISTANCE_INCHES = 10;
  51. const int MAX_SONIC_DISTANCE = 500;      // cm, optional, 500 cm is default
  52.  
  53. // Define 'ports' for motors.
  54. const byte LEFT_MOTOR_PORT = 3;
  55. const byte RIGHT_MOTOR_PORT = 1;
  56. // Create pointers to motor control objects
  57. Adafruit_DCMotor *motorLeft = AFMS.getMotor(LEFT_MOTOR_PORT);
  58. Adafruit_DCMotor *motorRight = AFMS.getMotor(RIGHT_MOTOR_PORT);
  59.  
  60. /* Define new untrasonic sensor with trigger & echo pins and
  61.    the maximum distance to be sensed. */
  62. NewPing sonic(SONIC_TRIGGER_PIN, SONIC_ECHO_PIN, MAX_SONIC_DISTANCE);
  63.  
  64. /* Define servo pins */
  65. const byte PANSERVOPIN = 10; // Servo 1 on AdaFruit Motor Shield
  66. const byte TILTSERVOPIN = 9; // Servo 2
  67.  
  68. float pingDist; // define variable to use for distance measurements
  69.  
  70.  
  71. void setup(void) {
  72.   AFMS.begin();  // create with the default frequency 1.6KHz
  73.   // Turn off all motors to start, just a good habit
  74.   motorLeft->run(RELEASE);
  75.   motorRight->run(RELEASE);
  76.  
  77.   panServo.attach(PANSERVOPIN);
  78.   tiltServo.attach(TILTSERVOPIN);
  79.   panServo.write(90);
  80.   tiltServo.write(60);
  81.  
  82.   Serial.begin(9600);  //Begin serial communcation
  83.  
  84.   /*Set up Bump Pins with Arduino internal pullup resistors
  85.     This will make them always high unless a bump switch is hit,
  86.     which will make a connection to ground and they will read low. */
  87.   pinMode(LEFT_BUMP_PIN, INPUT_PULLUP);
  88.   pinMode(RIGHT_BUMP_PIN, INPUT_PULLUP);
  89.  
  90.   pinMode(SONIC_TRIGGER_PIN, OUTPUT);
  91.   pinMode(SONIC_ECHO_PIN, INPUT);
  92.   //  delay(2000);  // Two second delay to get the robot placed
  93.   while (digitalRead(RIGHT_BUMP_PIN)) {}  // pause until right bump pin hit.
  94. }
  95.  
  96. void loop() {
  97.  
  98.   // Test some of the sonic library functions:
  99.   Serial.print(sonic.ping_in());
  100.   Serial.print(" inches, cm = ");
  101.   Serial.print(sonic.ping_cm());
  102.   Serial.print(", actual ping time (ms) = ");
  103.   int ping_milli = sonic.ping();
  104.   Serial.print(ping_milli);
  105.   Serial.print(", real inches = ");
  106.   Serial.print(Distance_inches(ping_milli));
  107.   Serial.print(", real cm = ");
  108.   Serial.println(Distance_cm(ping_milli));
  109.   // delay(100); // Just to slow things down
  110.  
  111.   /*   for (int i=0; i<=170; i+=5) {
  112.       panServo.write(i);
  113.       delay(50);
  114.     }
  115.     delay(1000);
  116.     for (int i=170; i>=0; i-=5) {
  117.       panServo.write(i);
  118.       delay(50);
  119.     }
  120.     delay(1000); */
  121.   //  panServo.write(180);
  122.   //  delay(1000);
  123.   //  panServo.write(90);
  124.   //  delay(1000);
  125.   //  panServo.write(0);
  126.   //   delay(1000);
  127.   //  panServo.write(90);
  128.  
  129.   // delay(1000);
  130.  
  131.   //   Assuming no switches closed initially.  Drive forward:
  132.   motorLeft->setSpeed(FORWARD_SPEED);
  133.   motorRight->setSpeed(FORWARD_SPEED);
  134.   pingDist = Distance_inches(sonic.ping());
  135.   while (pingDist == 0) {
  136.     pingDist = Distance_inches(sonic.ping());
  137.   }
  138.  
  139.  
  140.   Serial.println("First straight distance = " + String(pingDist));
  141.   while (digitalRead(LEFT_BUMP_PIN) && digitalRead(RIGHT_BUMP_PIN)
  142.          && pingDist > TARGET_DISTANCE_INCHES) {
  143.     motorLeft->run(FORWARD);
  144.     motorRight->run(FORWARD);
  145.     pingDist = Distance_inches(sonic.ping());
  146.     while (pingDist == 0) {
  147.       pingDist = Distance_inches(sonic.ping());
  148.     }
  149.     Serial.println("Straight distance = " + String(pingDist));
  150.     // Serial.println(String(pingDist));
  151.   }
  152.  
  153.   /*   If you got here, one of the bump switches was closed or B^3 is too
  154.        close to something straight ahead */
  155.  
  156.   /* First check the LEFT sensor: */
  157.   if (! digitalRead(LEFT_BUMP_PIN)) { // the LEFT side switch was bumped
  158.     motorLeft->setSpeed(BACKWARD_SPEED / 3); // Slowly back up and turn to right
  159.     motorRight->setSpeed(BACKWARD_SPEED);
  160.     motorLeft->run(BACKWARD);
  161.     motorRight->run(BACKWARD);
  162.     delay(TURN_DURATION);                  // for specified duration
  163.     motorLeft->run(RELEASE);               // Then stop power to the motors
  164.     motorRight->run(RELEASE);              // and move to next section of code
  165.   }
  166.   /* Then check the right sensor: */
  167.   else if (! digitalRead(RIGHT_BUMP_PIN)) { // the RIGHT side switch was bumped
  168.     motorLeft->setSpeed(BACKWARD_SPEED); // Slowly back up and turn to left
  169.     motorRight->setSpeed(BACKWARD_SPEED / 3);
  170.     motorLeft->run(BACKWARD);
  171.     motorRight->run(BACKWARD);
  172.     delay(TURN_DURATION);                 // for specified duration
  173.     motorLeft->run(RELEASE);               // Then stop power to the motors
  174.     motorRight->run(RELEASE);              // and move to next section of code
  175.   }
  176.   /* It must have been the sonar sensor */
  177.   else {
  178.     //    /* motorLeft->run(RELEASE);   // So stop power to the motors
  179.     //      motorRight->run(RELEASE);  // and move to next section of code */
  180.     //    motorLeft->run(BACKWARD);   // So REVERSE power to the motors
  181.     //    motorRight->run(BACKWARD);  // to BRIEFLY APPLY a brake
  182.     //    delay(50);
  183.     //    motorLeft->run(RELEASE);   // Then stop power to the motors
  184.     //    motorRight->run(RELEASE);  // and move to next section of code
  185.     allStop(FORWARD, motorLeft, motorRight);
  186.     float maxDist = -999.;
  187.     int newDirection = 0;
  188.  
  189.     for (int i = 20; i <= 160; i += 5) {
  190.       panServo.write(i);
  191.       // pingDist = max(Distance_inches(sonic.ping()),0.1);
  192.       pingDist = Distance_inches(sonic.ping());
  193.       while (pingDist == 0) {
  194.         pingDist = Distance_inches(sonic.ping());
  195.       }
  196.       if (pingDist > maxDist) {
  197.         //newDirection = i - 90;
  198.         newDirection = map(i, 0, 180, 90, -90);  //*****WHY THIS ORDER?*****
  199.         maxDist = pingDist;
  200.         String outMsg = String("New maximum distance is " +
  201.                                String(pingDist) + " at angle " + String(newDirection));
  202.         Serial.println(outMsg);
  203.       }
  204.       delay(50);
  205.     }
  206.     /* Now turn to the new maximumum direction! */
  207.     panServo.write(90);
  208.     spin(newDirection, 60, motorLeft, motorRight);
  209.     spinStop(newDirection, motorLeft, motorRight);
  210.  
  211.     motorLeft->run(RELEASE);   // Then stop power to the motors
  212.     motorRight->run(RELEASE);  // and move to next section of code
  213.     String outMsg = String("New direction is " + String(newDirection));
  214.     Serial.println(outMsg);
  215.     //    while (digitalRead(RIGHT_BUMP_PIN)) {}  // pause until right bump pin hit.
  216.   }
  217.   motorLeft->run(RELEASE);   // Then stop power to the motors
  218.   motorRight->run(RELEASE);  // and move to next section of code
  219.   panServo.write(90);
  220.   /*That is all!  Now go back to the beginning of the loop and
  221.      drive straight ahead until somehting is bumped. */
  222.  
  223. }
  224.  
  225. //int goodPing(void) {
  226. //  int goodPing = 0;
  227. //  int minBadPing = 0;
  228. //  int maxBadPing = 4.e6 * 2.0 / 13582.67; // uSecs to go and return 4 inches.
  229. //  goodPing = sonic.ping();
  230. //  while (goodPing >= minBadPing && goodPing <= maxBadPing) {
  231. //    goodPing = sonic.ping();
  232. //  }
  233. //  return goodPing;
  234. //}
  235.  
  236. float Distance_inches(int ping) {
  237.   float inches_per_sec = 13582.67;  // Equivilent to 345 m/s
  238.   return ping * inches_per_sec * 1e-6 * 0.5;
  239. }
  240.  
  241. float Distance_cm(int ping) {
  242.   float cm_per_sec = 34500.;        // Equivilent to 345 m/s
  243.   return ping * cm_per_sec * 1e-6 * 0.5;
  244. }
Success #stdin #stdout 0.02s 26000KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
#include <Adafruit_CircuitPlayground.h>

/* B3_09_Servo_SonicV2.ino

   Drive the TWO-WHEELED Bread Board Bot (BBbot, B^3)
   forward.   When within 10 inches of something, turn servos to find clearest path
   and then go that way.
   When a whisker bump sensor on either side hits something,
   back up and turn slightly away from that direction and resume
   forward path.

   Stop at a specified distance from an object directly ahead.

   Arduino: Arduino Mega 256 v3 Clone
   Motor Shield: Adafruit assembled Motor Shield for Arduino v2
   ---->  http://w...content-available-to-author-only...t.com/products/1438

   Programmer: Dave Eslinger; June 13, 2015
   Revisions:   Added sonic sensors on servos, June 23, 2016.
*/
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <math.h>
#include <BreadBoardBot.h>
#include <NewPing.h>
#include <Servo.h>

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Define Servos
Servo panServo;
Servo tiltServo;

/* Define Constants */

// IO Pins used
const byte LEFT_BUMP_PIN = 47;    // Define DIGITAL Pins for left
const byte RIGHT_BUMP_PIN = 46;   // and right bump sensors
const byte SONIC_TRIGGER_PIN = 51;
const byte SONIC_ECHO_PIN = 50;

// Parameters controlling program behavior
// Bump behavior
const byte FORWARD_SPEED = 100;   // Define normal speeds
const byte BACKWARD_SPEED = 60;  // and backup/turn speed
const int  TURN_DURATION = 600;   // Turn length in milliseconds

// Sonic sensor
const float TARGET_DISTANCE_INCHES = 10;
const int MAX_SONIC_DISTANCE = 500;      // cm, optional, 500 cm is default

// Define 'ports' for motors.
const byte LEFT_MOTOR_PORT = 3;
const byte RIGHT_MOTOR_PORT = 1;
// Create pointers to motor control objects
Adafruit_DCMotor *motorLeft = AFMS.getMotor(LEFT_MOTOR_PORT);
Adafruit_DCMotor *motorRight = AFMS.getMotor(RIGHT_MOTOR_PORT);

/* Define new untrasonic sensor with trigger & echo pins and
   the maximum distance to be sensed. */
NewPing sonic(SONIC_TRIGGER_PIN, SONIC_ECHO_PIN, MAX_SONIC_DISTANCE);

/* Define servo pins */
const byte PANSERVOPIN = 10; // Servo 1 on AdaFruit Motor Shield
const byte TILTSERVOPIN = 9; // Servo 2

float pingDist; // define variable to use for distance measurements


void setup(void) {
  AFMS.begin();  // create with the default frequency 1.6KHz
  // Turn off all motors to start, just a good habit
  motorLeft->run(RELEASE);
  motorRight->run(RELEASE);

  panServo.attach(PANSERVOPIN);
  tiltServo.attach(TILTSERVOPIN);
  panServo.write(90);
  tiltServo.write(60);

  Serial.begin(9600);  //Begin serial communcation

  /*Set up Bump Pins with Arduino internal pullup resistors
    This will make them always high unless a bump switch is hit,
    which will make a connection to ground and they will read low. */
  pinMode(LEFT_BUMP_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BUMP_PIN, INPUT_PULLUP);

  pinMode(SONIC_TRIGGER_PIN, OUTPUT);
  pinMode(SONIC_ECHO_PIN, INPUT);
  //  delay(2000);  // Two second delay to get the robot placed
  while (digitalRead(RIGHT_BUMP_PIN)) {}  // pause until right bump pin hit.
}

void loop() {

  // Test some of the sonic library functions:
  Serial.print(sonic.ping_in());
  Serial.print(" inches, cm = ");
  Serial.print(sonic.ping_cm());
  Serial.print(", actual ping time (ms) = ");
  int ping_milli = sonic.ping();
  Serial.print(ping_milli);
  Serial.print(", real inches = ");
  Serial.print(Distance_inches(ping_milli));
  Serial.print(", real cm = ");
  Serial.println(Distance_cm(ping_milli));
  // delay(100); // Just to slow things down

  /*   for (int i=0; i<=170; i+=5) {
      panServo.write(i);
      delay(50);
    }
    delay(1000);
    for (int i=170; i>=0; i-=5) {
      panServo.write(i);
      delay(50);
    }
    delay(1000); */
  //  panServo.write(180);
  //  delay(1000);
  //  panServo.write(90);
  //  delay(1000);
  //  panServo.write(0);
  //   delay(1000);
  //  panServo.write(90);

  // delay(1000);

  //   Assuming no switches closed initially.  Drive forward:
  motorLeft->setSpeed(FORWARD_SPEED);
  motorRight->setSpeed(FORWARD_SPEED);
  pingDist = Distance_inches(sonic.ping());
  while (pingDist == 0) {
    pingDist = Distance_inches(sonic.ping());
  }


  Serial.println("First straight distance = " + String(pingDist));
  while (digitalRead(LEFT_BUMP_PIN) && digitalRead(RIGHT_BUMP_PIN)
         && pingDist > TARGET_DISTANCE_INCHES) {
    motorLeft->run(FORWARD);
    motorRight->run(FORWARD);
    pingDist = Distance_inches(sonic.ping());
    while (pingDist == 0) {
      pingDist = Distance_inches(sonic.ping());
    }
    Serial.println("Straight distance = " + String(pingDist));
    // Serial.println(String(pingDist));
  }

  /*   If you got here, one of the bump switches was closed or B^3 is too
       close to something straight ahead */

  /* First check the LEFT sensor: */
  if (! digitalRead(LEFT_BUMP_PIN)) { // the LEFT side switch was bumped
    motorLeft->setSpeed(BACKWARD_SPEED / 3); // Slowly back up and turn to right
    motorRight->setSpeed(BACKWARD_SPEED);
    motorLeft->run(BACKWARD);
    motorRight->run(BACKWARD);
    delay(TURN_DURATION);                  // for specified duration
    motorLeft->run(RELEASE);               // Then stop power to the motors
    motorRight->run(RELEASE);              // and move to next section of code
  }
  /* Then check the right sensor: */
  else if (! digitalRead(RIGHT_BUMP_PIN)) { // the RIGHT side switch was bumped
    motorLeft->setSpeed(BACKWARD_SPEED); // Slowly back up and turn to left
    motorRight->setSpeed(BACKWARD_SPEED / 3);
    motorLeft->run(BACKWARD);
    motorRight->run(BACKWARD);
    delay(TURN_DURATION);                 // for specified duration
    motorLeft->run(RELEASE);               // Then stop power to the motors
    motorRight->run(RELEASE);              // and move to next section of code
  }
  /* It must have been the sonar sensor */
  else {
    //    /* motorLeft->run(RELEASE);   // So stop power to the motors
    //      motorRight->run(RELEASE);  // and move to next section of code */
    //    motorLeft->run(BACKWARD);   // So REVERSE power to the motors
    //    motorRight->run(BACKWARD);  // to BRIEFLY APPLY a brake
    //    delay(50);
    //    motorLeft->run(RELEASE);   // Then stop power to the motors
    //    motorRight->run(RELEASE);  // and move to next section of code
    allStop(FORWARD, motorLeft, motorRight);
    float maxDist = -999.;
    int newDirection = 0;

    for (int i = 20; i <= 160; i += 5) {
      panServo.write(i);
      // pingDist = max(Distance_inches(sonic.ping()),0.1);
      pingDist = Distance_inches(sonic.ping());
      while (pingDist == 0) {
        pingDist = Distance_inches(sonic.ping());
      }
      if (pingDist > maxDist) {
        //newDirection = i - 90;
        newDirection = map(i, 0, 180, 90, -90);  //*****WHY THIS ORDER?*****
        maxDist = pingDist;
        String outMsg = String("New maximum distance is " +
                               String(pingDist) + " at angle " + String(newDirection));
        Serial.println(outMsg);
      }
      delay(50);
    }
    /* Now turn to the new maximumum direction! */
    panServo.write(90);
    spin(newDirection, 60, motorLeft, motorRight);
    spinStop(newDirection, motorLeft, motorRight);

    motorLeft->run(RELEASE);   // Then stop power to the motors
    motorRight->run(RELEASE);  // and move to next section of code
    String outMsg = String("New direction is " + String(newDirection));
    Serial.println(outMsg);
    //    while (digitalRead(RIGHT_BUMP_PIN)) {}  // pause until right bump pin hit.
  }
  motorLeft->run(RELEASE);   // Then stop power to the motors
  motorRight->run(RELEASE);  // and move to next section of code
  panServo.write(90);
  /*That is all!  Now go back to the beginning of the loop and
     drive straight ahead until somehting is bumped. */

}

//int goodPing(void) {
//  int goodPing = 0;
//  int minBadPing = 0;
//  int maxBadPing = 4.e6 * 2.0 / 13582.67; // uSecs to go and return 4 inches.
//  goodPing = sonic.ping();
//  while (goodPing >= minBadPing && goodPing <= maxBadPing) {
//    goodPing = sonic.ping();
//  }
//  return goodPing;
//}

float Distance_inches(int ping) {
  float inches_per_sec = 13582.67;  // Equivilent to 345 m/s
  return ping * inches_per_sec * 1e-6 * 0.5;
}

float Distance_cm(int ping) {
  float cm_per_sec = 34500.;        // Equivilent to 345 m/s
  return ping * cm_per_sec * 1e-6 * 0.5;
}
https://ideone.com/cYPU34
language:
PHP (php 7.3.5)
created:
2 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!