ultrasonic.ino

#include <Servo.h>
Servo myservo;                                      // create servo object to control a servo 
 
int pos=0;                                          // variable to store the servo position
int URPWM=3;                                        // PWM Output 0-25000us,every 50us represent 1cm
int URTRIG=5;                                       // PWM trigger pin
boolean up=true;                                    // create a boolean variable
unsigned long time;                                 // create a time variable
unsigned long urmTimer = 0;                          // timer for managing the sensor reading flash rate
 
unsigned int Distance=0;
uint8_t EnPwmCmd[4]={0x44,0x22,0xbb,0x01};          // distance measure command
 

int speedPin_M1 = 5;     //M1 Speed Control
int speedPin_M2 = 6;     //M2 Speed Control
int directionPin_M1 = 4;     //M1 Direction Control
int directionPin_M2 = 7;     //M1 Direction Control

void setup() {                                // Serial initialization

  Serial.begin(9600);                         // Sets the baud rate to 9600
myservo.attach(9);                                // Pin 9 to control servo
  PWM_Mode_Setup();
  //pinMode(speedPin_M1, OUTPUT);
}

void loop() {
//   if(millis()-time>=20){                           // interval 0.02 seconds
//     time=millis();                                 // get the current time of programme
//     if(up){                                        // judge the condition
//      if(pos>=0 && pos<=179){                   
//        pos=pos+1;                                  // in steps of 1 degree 
//        myservo.write(pos);                         // tell servo to go to position in variable 'pos' 
//      }
//      if(pos>179)  up= false;                       // assign the variable again
//    }
//     else {                                       
//      if(pos>=1 && pos<=180){    
//        pos=pos-1;
//        myservo.write(pos);
//      }
//      if(pos<1)  up=true;
//    }
//   }
    
   if(millis()-urmTimer>50){
     urmTimer=millis(); 
     PWM_Mode();
   }

   if (Distance >=50) {
  carAdvance(200, 200);
   }
   else {
    carTurnLeft(200,200);
   }
  
//  delay(1000);
//  carTurnRight(300, 300);
//  //                 Serial.println("carTurnRight");
//  delay(1000);
//  carAdvance(-200,-200);
//  delay(1000);
//  carTurnLeft(200,200);
//  carStop();

}

void PWM_Mode_Setup(){ 
  pinMode(URTRIG,OUTPUT);                            // A low pull on pin COMP/TRIG
  digitalWrite(URTRIG,HIGH);                         // Set to HIGH
  
  pinMode(URPWM, INPUT);                             // Sending Enable PWM mode command
  
  for(int i=0;i<4;i++){
      Serial.write(EnPwmCmd[i]);
   } 
}
 
void PWM_Mode(){                                     // a low pull on pin COMP/TRIG  triggering a sensor reading
    digitalWrite(URTRIG, LOW); 
    digitalWrite(URTRIG, HIGH);                      // reading Pin PWM will output pulses
     
    unsigned long DistanceMeasured=pulseIn(URPWM,LOW);
     
    if(DistanceMeasured==50000){                     // the reading is invalid.
      Serial.print("Invalid");    
   }
    else{
      Distance=DistanceMeasured/50;                  // every 50us low level stands for 1cm
   }
  Serial.print("Distance=");
  Serial.print(Distance);
  Serial.println("cm");
}


void carStop() {                //  Motor Stop
  digitalWrite(speedPin_M2, 0);
  digitalWrite(directionPin_M1, LOW);
  digitalWrite(speedPin_M1, 0);
  digitalWrite(directionPin_M2, LOW);
}

void carAdvance(int leftSpeed, int rightSpeed) {       //Move forward
  analogWrite (speedPin_M2, leftSpeed);             //PWM Speed Control
  digitalWrite(directionPin_M1, HIGH);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, HIGH);
}

//void carBack(int leftSpeed, int rightSpeed) {     //Move backward
//  analogWrite (speedPin_M2, leftSpeed);
//  digitalWrite(directionPin_M1, LOW);
//  analogWrite (speedPin_M1, rightSpeed);
//  digitalWrite(directionPin_M2, LOW);
//}

void carTurnRight(int leftSpeed, int rightSpeed) {           //Turn Left
  analogWrite (speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, LOW);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, HIGH);
}
void carTurnLeft(int leftSpeed, int rightSpeed) {         //Turn Right
  analogWrite (speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, HIGH);
  analogWrite (speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, LOW);
}