ตัวอย่างโค้ด
#define echoPin 6 // Echo Pin
#define trigPin 5 // Trigger Pin
int incomingByte = 2; // for incoming serial data
// Motor-controller connections to Arduino
int motor1_IN1 = 2;
int motor1_IN2 = 4;
int motor2_IN3 = 7;
int motor2_IN4 = 8;
int motor1_BLI = 3; // m1_B low-side (N-channel mosfet) PWM output
int motor1_ALI = 11; // m1_A low-side (N-channel mosfet) PWM output
int speed_motor = 150;
int ledPin1 = 13; // used as a neutral indicator
#include <Servo.h>
Servo myservo; // create servo object to control a servo
// a maximum of eight servo objects can be created
long duration, inches, cm,checkl;
int pos = 0; // variable to store the servo position
int R_Angle = 0;
int C_Angle = 90;
int L_Angle = 180;
// value names used to hold antennae state
int side_R_val;
int side_L_val;
int side_C_val;
void setup()
{
TCCR2B = TCCR2B & 0b11111000 | 0x01; // change PWM frequency on pins 3 and 11 to 32kHz
myservo.attach(9); // attaches the servo on pin 9 to the servo object
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
//motor1 pins
pinMode(motor1_ALI, OUTPUT);
pinMode(motor1_BLI, OUTPUT);
//motor2 pins
pinMode(motor1_IN1, OUTPUT);
pinMode(motor1_IN2, OUTPUT);
pinMode(motor2_IN3, OUTPUT);
pinMode(motor2_IN4, OUTPUT);
//led's
pinMode(ledPin1, OUTPUT);
}
void loop()
{
checkl = microsecondsToCentimeters();
if (checkl<=10){
stop_motors();
myservo.write(L_Angle);
delay(500);
side_L_val = microsecondsToCentimeters();
Serial.print(side_L_val);
Serial.println();
delay(500);
myservo.write(C_Angle);
delay(500);
side_C_val = microsecondsToCentimeters();
Serial.print(side_C_val);
Serial.println();
delay(500);
myservo.write(R_Angle);
delay(500);
side_R_val = microsecondsToCentimeters();
Serial.print(side_R_val);
Serial.println();
delay(500);
if (side_L_val > side_R_val){
myservo.write(C_Angle);
turn_left();
}else{
myservo.write(C_Angle);
turn_right();
}
}else
{
forward_motors();
}
}
// Beginning motor control functions
void stop_motors(){
m1_stop();
m2_stop();
Serial.print("stop motors");
Serial.println();
}
void turn_right(){
Serial.print("turn right");
Serial.println();
// delay(2000);
reverse_motors();
delay(350);
m1_forward(speed_motor);
delay(500);
m2_stop();
}
void turn_left(){
Serial.print("turn left");
Serial.println();
// delay(2000);
reverse_motors();
delay(350);
m2_forward(speed_motor);
delay(500);
m1_stop();
}
void reverse_motors(){
m1_reverse(speed_motor);
m2_reverse(speed_motor);
Serial.print("reverse motors");
Serial.println();
}
void forward_motors(){
m1_forward(speed_motor);
m2_forward(speed_motor);
Serial.print("forward motors");
Serial.println();
}
// Create single instances for each motor direction, so we don't accidentally write a shoot-
void m1_forward(int m1_speed){
digitalWrite(motor1_IN1, HIGH);
digitalWrite(motor1_IN2, LOW);
analogWrite(motor1_ALI, m1_speed);
digitalWrite(ledPin1, LOW);
}
void m1_reverse(int m1_speed){
digitalWrite(motor1_IN1, LOW);
digitalWrite(motor1_IN2, HIGH);
analogWrite(motor1_ALI, m1_speed);
digitalWrite(ledPin1, LOW);
}
void m2_forward(int m2_speed){
digitalWrite(motor2_IN3, HIGH);
digitalWrite(motor2_IN4, LOW);
analogWrite(motor1_BLI, m2_speed);
digitalWrite(ledPin1, LOW);
}
void m2_reverse(int m2_speed){
digitalWrite(motor2_IN3, LOW);
digitalWrite(motor2_IN4, HIGH);
analogWrite(motor1_BLI, m2_speed);
digitalWrite(ledPin1, LOW);
}
void m1_stop(){
digitalWrite(motor1_IN1, LOW);
digitalWrite(motor1_IN2, LOW);
digitalWrite(ledPin1, HIGH);
}
void m2_stop(){
digitalWrite(motor2_IN3, LOW);
digitalWrite(motor2_IN4, LOW);
digitalWrite(ledPin1, HIGH);
}
// End motor functions
long microsecondsToCentimeters()
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
return duration / 29 / 2;;
}
สร้างหุ่นยนต์ด้วย Arduino ตอนที่ 3 ฝึกให้หุ่นยนต์หลบหลีกสิ่งกีดขวาง
http://www.instructables.com/id/Simple-RC-car-for-beginners-Android-control-over-/step10/Software/
ไม่มีความคิดเห็น:
แสดงความคิดเห็น