Go Down

Topic: ultrasonic can not work with Adafruit Motor Shield (Read 279 times) previous topic - next topic

j1182478

I used 3 hc-sr04 , Adafruit Motor Shield and arduino mega 2569.
One of hc-sr04 can not work.
But them can work without Adafruit Motor Shield.

Code: [Select]
#include <NewPing.h>
#include <SoftwareSerial.h>
#include <AFMotor.h>

//SoftwareSerial mySerial(32,33); // RX, TX

#define SONAR_NUM     3 // Number or sensors.
#define MAX_DISTANCE 300 // Max distance in cm.
#define PING_INTERVAL 66// Milliseconds between pings.

char val;
byte dir = 0;  

unsigned long pingTimer[SONAR_NUM]; // When each pings.
unsigned int cm[SONAR_NUM]; // Store ping distances.
uint8_t currentSensor = 0; // Which sensor is active.

NewPing sonar[SONAR_NUM] = { // Sensor object array.
  NewPing (30, 31, MAX_DISTANCE),
  NewPing (40, 41, MAX_DISTANCE),
  NewPing (50, 51, MAX_DISTANCE)
};

AF_DCMotor leftMotor2(1, MOTOR12_64KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor rightMotor2(2, MOTOR12_64KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor1(3, MOTOR34_1KHZ);// create motor #3, using M3 output, set to 1kHz PWM frequency
AF_DCMotor leftMotor1(4, MOTOR34_1KHZ);// create motor #4, using M4 output, set to 1kHz PWM frequency

String motorSet = "";
const int dangerThresh = 15;

void setup() {
  Serial.begin(115200);
  //mySerial.begin(9600);
  leftMotor1.setSpeed(200);    
  rightMotor2.setSpeed(200);
  rightMotor1.setSpeed(200);
  leftMotor2.setSpeed(200);//可調轉速約150~到255
  //pinMode(IA, OUTPUT);
  //pinMode(IB, OUTPUT);
  pingTimer[0] = millis() + 75; // First ping start in ms.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void stop() {leftMotor1.run(RELEASE); leftMotor2.run(RELEASE); rightMotor1.run(RELEASE); rightMotor2.run(RELEASE);}  // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void forward() {
    motorSet = "FORWARD";
    leftMotor1.run(FORWARD);      // turn it on going forward
    leftMotor2.run(FORWARD);      // turn it on going forward
    rightMotor1.run(FORWARD);     // turn it on going forward
    rightMotor2.run(FORWARD);     // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void backward() {
    motorSet = "BACKWARD";
    leftMotor1.run(BACKWARD);     // turn it on going backward
    leftMotor2.run(BACKWARD);     // turn it on going backward
    rightMotor1.run(BACKWARD);    // turn it on going backward
    rightMotor2.run(BACKWARD);    // turn it on going backward
}  
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
  motorSet = "RIGHT";
  leftMotor1.run(BACKWARD);      // turn motor 1 forward
  leftMotor2.run(BACKWARD);      // turn motor 2 forward
  rightMotor1.run(FORWARD);    // turn motor 3 backward
  rightMotor2.run(FORWARD);    // turn motor 4 backward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
  motorSet = "LEFT";
  leftMotor1.run(FORWARD);      // turn motor 1 backward
  leftMotor2.run(FORWARD);      // turn motor 2 backward
  //leftMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);    
  //leftMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);    
  rightMotor1.run(BACKWARD);     // turn motor 3 forward
  rightMotor2.run(BACKWARD);     // turn motor 4 forward
}  
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
  motorSet = "RIGHT";
  leftMotor1.run(BACKWARD);      // turn motor 1 forward
  leftMotor2.run(BACKWARD);      // turn motor 2 forward
  rightMotor1.run(FORWARD);    // turn motor 3 backward
  rightMotor2.run(FORWARD);    // turn motor 4 backward
}  

void echoCheck() { // If ping echo, set distance to array.
 // Serial.println("echoCheck");
 // Serial.println(currentSensor);
  if (sonar[currentSensor].check_timer())  
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
  // Serial.print("cm x ");
  // Serial.println(cm[currentSensor]);
}

void oneSensorCycle() { // Do something with the results.
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
      Serial.print("ping");
      Serial.print(i);
      Serial.print("=");
      Serial.print(cm[i]);
      Serial.print("cm ");
      //Serial.print(sonar[currentSensor].ping_result / US_ROUNDTRIP_CM);
  }
  //Serial.print(cm[currentSensor]);
  Serial.println();
//------------------------------------------------------------------------------------------
   if (cm[0] > dangerThresh && cm[1] > dangerThresh && cm[2] > dangerThresh){ //0 0 0
      Serial.println("0 0 0");
      if (dir != 0) {    
         dir = 0;
         stop();  
         delay(50);
      }
      forward();
      delay(100);  
   }
   else if(cm[0] > dangerThresh && cm[1] > dangerThresh && cm[2] < dangerThresh){ //0 0 1
          Serial.println("0 0 1");
          if (dir != 1) {    
             dir = 1;
             stop();  
             delay(50);
          }
          turnLeft();
          delay(100);  
   }
   else if(cm[0] > dangerThresh && cm[1] < dangerThresh && cm[2] < dangerThresh){ //0 1 1
          Serial.println("0 1 1");
          if (dir != 1) {      
             dir = 1;
             stop();  
             delay(50);
          }
          turnLeft();
          delay(100);  
   }
   else if(cm[0] < dangerThresh && cm[1] > dangerThresh && cm[2] > dangerThresh){ //1 0 0
          Serial.println("1 0 0");
          if (dir != 1) {  
             dir = 1;
             stop();  
             delay(50);
          }
          turnRight();
          delay(100);  
   }
   else if(cm[0] < dangerThresh && cm[1] < dangerThresh && cm[2] > dangerThresh){ //1 1 0
          Serial.println("1 1 0");
          if (dir != 1) {    
             dir = 1;
             stop();  
             delay(50);
          }
          turnRight();
          delay(100);  
   }
   else if(cm[0] > dangerThresh && cm[1] < dangerThresh && cm[2] > dangerThresh){ //0 1 0
          Serial.println("0 1 0");
          if (dir != 1) {      
             dir = 1;
             stop();  
             delay(50);
          }
          turnLeft();
          delay(100);  
   }
   else if(cm[0] > dangerThresh && cm[1] < dangerThresh && cm[2] < dangerThresh){ //1 0 1
          Serial.println("1 0 1");
          if (dir != 0) {      
             dir = 0;
             stop();  
             delay(50);
          }
          forward();
          delay(100);  
   }
   else if(cm[0] < dangerThresh && cm[1] < dangerThresh && cm[2] < dangerThresh){ //1 1 1
          Serial.println("1 1 1");
          if (dir != 1) {      
             dir = 1;
             stop();  
             delay(50);
          }
          backward();
          delay(100);
   }
//--------------------------------------------------------------------------------------------  
}  

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    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).
      
      // Serial.print("i = ");
      // Serial.println(i);
      // Serial.print("currentSensor =");
      // Serial.println(currentSensor);
      //Serial.println("currentSensor = %d",currentSensor);
    }
  }      
}

Go Up