Я разрабатываю простого робота (с 4-х колесной базой), который движется вперед и обнаруживает объект с помощью датчика расстояния, берет его рукой. Объект представляет собой коробку с какой-либо ручкой, направленной вверх, поэтому рука помещается ниже, так называемой, ручки объекта и поднимает его вверх. Задача, которую я хочу достичь, заключается в том, что робот должен двигаться вперед и помещать объект в контейнер (это также физический объект), размещенный в каком-то указанном месте.
Робот движется с помощью 4 двигателей постоянного тока, установленных на колесах его основания, а рука управляется отдельным двигателем постоянного тока.
Я хочу, чтобы:
Робот должен двигаться вперед, пока не обнаружит объект.
Когда объект обнаружен, он должен остановить колеса и запустить рычаг, чтобы поднять объект вверх.
Затем он должен двигаться вперед, пока не обнаружит второй объект, то есть контейнер.
При обнаружении второго объекта (контейнера) колеса должны быть остановлены, а рычаг должен быть активирован, чтобы поместить объект в контейнер.
Для этого я написал следующий код
#define mp1 3
#define mp2 4
#define m2p1 5
#define m2p2 6
#define echoPin 7 // Echo Pin
#define trigPin 8 // Trigger Pin
#define LEDPin 13 // Onboard LED
#define armPin1 9 // Pin 1 of arm
#define armPin2 10 // Pin 2 of arm
int maximumRange = 200; // Maximum range needed
int minimumRange = 18; // Minimum range needed
long duration, distance; // Duration used to calculate distance
int first = 0;
void setup()
{
Serial.begin(9600);
//Setting the pins of motors
pinMode(mp1, OUTPUT);
pinMode(mp2, OUTPUT);
pinMode(m2p1, OUTPUT);
pinMode(m2p2, OUTPUT);
//Setting the pins of distance sensor
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(LEDPin, OUTPUT); // Use LED indicator (if required)
pinMode(armPin1, OUTPUT);
pinMode(armPin2, OUTPUT);
}// end setup method
long calculateDistance(){
//Code of distance sensor
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(20);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
//Calculate the distance (in cm) based on the speed of sound.
return duration/58.2;
}
void loop()
{
distance = calculateDistance();
while(distance > minimumRange) {
forward();
distance = calculateDistance();
}
while(distance <= minimumRange && first == 0){
stopMotors();
pickObject();
distance = calculateDistance();
}
while(distance > minimumRange) {
forward();
distance = calculateDistance();
}
while(distance <= minimumRange && first == 1){
stopMotors();
putObject();
distance = calculateDistance();
}// end second while copied
first = 1;
}// end loop function
void pickObject(){
digitalWrite(armPin1, LOW);
digitalWrite(armPin2, HIGH);
}
void putObject(){
digitalWrite(armPin1, HIGH);
digitalWrite(armPin2, LOW);
}
void stopMotors(){
digitalWrite(mp1, LOW);
digitalWrite(mp2, LOW);
digitalWrite(m2p1, LOW);
digitalWrite(m2p2, LOW);
}
void forward(){
digitalWrite(mp1, LOW);
digitalWrite(mp2, HIGH);
digitalWrite(m2p1, HIGH);
digitalWrite(m2p2, LOW);
}
Но код не работает так, как я хочу. Это фактически перемещает руку в направлении вниз. Это может быть потому, что я не понял поток функции loop().
Может ли кто-нибудь сказать, в чем проблема в моем коде и каким должен быть мой код, чтобы получить желаемый результат?
