6个超声波传感器旋转伺服电机

时间:2013-06-05 11:33:55

标签: arduino

对于我们的移动镜子项目,我们使用6个超声波传感器来旋转镜子所在的伺服电机。

我们的理念是,如果一个传感器检测到他们前面的某个人/某物,那么他们就会远离那个人。虽然我似乎无法拒绝它。

我使用过“NewPing”库,可以在https://code.google.com/p/arduino-new-ping/看到,我使用了15传感器示例(https://code.google.com/p/arduino-new-ping/wiki/15_Sensors_Example

这是正确的编码方式还是应该从头开始并使用其他方法?

提前致谢!

守则:

// ---------------------------------------------------------------------------
// This example code was used to successfully communicate with 15 ultrasonic sensors. You can adjust
// the number of sensors in your project by changing SONAR_NUM and the number of NewPing objects in the
// "sonar" array. You also need to change the pins for each sensor for the NewPing objects. Each sensor
// is pinged at 33ms intervals. So, one cycle of all sensors takes 495ms (33 * 15 = 495ms). The results
// are sent to the "oneSensorCycle" function which currently just displays the distance data. Your project
// would normally process the sensor results in this function (for example, decide if a robot needs to
// turn and call the turn function). Keep in mind this example is event-driven. Your complete sketch needs
// to be written so there's no "delay" commands and the loop() cycles at faster than a 33ms rate. If other
// processes take longer than 33ms, you'll need to increase PING_INTERVAL so it doesn't get behind.
// ---------------------------------------------------------------------------
// Sketch of 6 sensors with servo
// ---------------------------------------------------------------------------
// |            \               |               |             /              |
// |        -----------    -----------    -----------    -----------         |
// |        |Sensor 2 |    |Sensor 3 |    |Sensor 4 |    |Sensor 5 |         |
// |  \     -----------    -----------    -----------    -----------         |
// |   \                                                              /      |
// |  -----------                                               -----------  |
// |  |Sensor 1 |                                               |Sensor 6 |  |
// |  -----------                -----------                    -----------  |
// |                             |  Servo  |                                 |
// |                             -----------                                 |
// |                                                                         |
// |   if Sensor 1 measures someone close the Servo should turn away from    |
// |                         Sensor 1                                        |
// ---------------------------------------------------------------------------
#include <NewPing.h>

#include <Servo.h>

Servo myservo;

#define SONAR_NUM     6 // Number or sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).

unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.
int val;                            // The number which makes the Servomotor rotate

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  NewPing(1, 2, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(3, 4, MAX_DISTANCE),
  NewPing(5, 6, MAX_DISTANCE),
  NewPing(7, 8, MAX_DISTANCE),
  NewPing(9, 10, MAX_DISTANCE),
  NewPing(11, 12, MAX_DISTANCE)
  };

void setup() {
  Serial.begin(115200);
  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
    if (millis() >= pingTimer[i]) {         // Is it this sensor's time to ping?
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
      currentSensor = i;                          // Sensor being accessed.
      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
    }
  }
  // -------------- BEGIN SELFMADE CODE -----------------------
  if(cm[1] <= 30) {
    val = 60;
  } 
  if(cm[2] <= 30) {
    val = 100;
  } 
  if(cm[3] <= 30) {
    val = 140;
  } 
  if(cm[4] <= 30) {
    val = 120;
  } 
  if(cm[5] <= 30) {
    val = 80;
  } 
  if(cm[6] <= 30) {
    val = 40;
  } 
  myservo.write(val);
  // -------------- END SELFMADE CODE --------------------
}

void echoCheck() { // If ping received, set the sensor distance to array.
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    Serial.print(i);
    Serial.print("=");
    Serial.print(cm[i]);
    Serial.print("cm ");
  }
  Serial.println();
}

1 个答案:

答案 0 :(得分:0)

这就是我构建代码的方式。

我刚将自制零件移动到 oneSensorCycle()中。我不喜欢它使用地球仪的方式,但它仍然看起来很干净。

#include <NewPing.h>

#include <Servo.h>

Servo myservo;

#define SONAR_NUM     6 // Number or sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).

unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen     for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.
//int val;                            // The number which makes the Servomotor rotate

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
NewPing(1, 2, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(3, 4, MAX_DISTANCE),
NewPing(5, 6, MAX_DISTANCE),
NewPing(7, 8, MAX_DISTANCE),
NewPing(9, 10, MAX_DISTANCE),
NewPing(11, 12, MAX_DISTANCE)
};

void setup() {
Serial.begin(115200);
pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the  Arduino to chill before starting.
 for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
      pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
 }

 void loop() {
 for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
   if (millis() >= pingTimer[i]) {         // Is it this sensor's time to ping?
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
      currentSensor = i;                          // Sensor being accessed.
      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
  }
}
}

 void echoCheck() { // If ping received, set the sensor distance to array.
 if (sonar[currentSensor].check_timer())
  cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
 }

 void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
 int val =0;
 for (uint8_t i = 0; i < SONAR_NUM; i++) {
  Serial.print(i);
  Serial.print("=");
  Serial.print(cm[i]);
  Serial.print("cm ");
// -------------- BEGIN SELFMADE CODE -----------------------

if(cm[0] <= 30) {
  val = 60;
} 
if(cm[1] <= 30) {
  val = 100;
} 
if(cm[2] <= 30) {
  val = 140;
} 
if(cm[3] <= 30) {
  val = 120;
} 
if(cm[4] <= 30) {
  val = 80;
} 
if(cm[5] <= 30) {
  val = 40;
} 
myservo.write(val);

// -------------- END SELFMADE CODE -----------------------


}
Serial.println();
}