I am able to control 2 DC motors with the pulseIn command and with an rc receiver. My problem is that when the - if (digitalRead(52) == LOW) - is triggered on or off by a switch on the receiver. the code should switch to an autonomous mode that I want to be controlled by ultrasonic sensors. However when this happens the motors move then stop repeatedly causing an unwanted twitching effect, I believe the pulseIn may be causing this but i am still unsure.
const int rc = A1;
const int rc2 = A0;
const int Forward1 = 10;
const int Backward2 = 9;
const int Forward3 = 8;
const int Backward4 = 7;
int duration;
int duration2;
double fnc_ultrasonic_distance(int _t, int _e){
unsigned long dur=0;
digitalWrite(_t, LOW);
delayMicroseconds(5);
digitalWrite(_t, HIGH);
delayMicroseconds(10);
digitalWrite(_t, LOW);
dur = pulseIn(_e, HIGH, 18000);
if(dur==0)return 999.0;
return (dur/57);
}
void setup() {
pinMode(rc, INPUT);
pinMode(rc2, INPUT);
pinMode(Forward1, OUTPUT);
pinMode(Backward2, OUTPUT);
digitalWrite(Forward1, LOW);
digitalWrite(Backward2, LOW);
pinMode(Forward3, OUTPUT);
pinMode(Backward4, OUTPUT);
digitalWrite(Forward3, LOW);
digitalWrite(Backward4, LOW);
pinMode(24, OUTPUT);
pinMode(22, INPUT);
}
void loop() {
if (digitalRead(52) == LOW) {
duration = constrain(pulseIn(rc, HIGH), 1000, 2000);
duration2 = constrain(pulseIn(rc2, HIGH), 1000, 2000);
if (duration <= 1600 && duration >= 1400) {
analogWrite(Forward1, 0);
analogWrite(Backward2, 0);
analogWrite(Forward3, 0);
analogWrite(Backward4, 0);
}
if (duration > 1600) {
analogWrite(Forward1, 0);
analogWrite(Backward2, map(duration,1600, 2000, 180, 255));
analogWrite(Forward3, 0);
analogWrite(Backward4, map(duration,1600, 2000, 180, 255));
}
if (duration2 > 1600) {
analogWrite(Forward1, map(duration2,1600, 2000, 180, 255));
analogWrite(Backward2, 0);
analogWrite(Forward3, 0);
analogWrite(Backward4, map(duration2,1600, 2000, 180, 255));
}
if (duration < 1400) {
analogWrite(Forward1, map(duration,1400, 1000, 180 , 255));
analogWrite(Backward2,0);
analogWrite(Forward3, map(duration,1400, 1000, 180 , 255));
analogWrite(Backward4,0);
}
if (duration2 < 1400) {
analogWrite(Forward1, 0);
analogWrite(Backward2,map(duration2,1400, 1000, 180 , 255));
analogWrite(Forward3, map(duration2,1400, 1000, 180 , 255));
analogWrite(Backward4,0);
}
}
if (digitalRead(52) == HIGH) {
if ((fnc_ultrasonic_distance(24,22) <= 20)) {
analogWrite(Forward1, 100);
analogWrite(Backward2,0);
analogWrite(Forward3, 100);
analogWrite(Backward4,0);
} else {
analogWrite(Forward1, 0);
analogWrite(Backward2,100);
analogWrite(Forward3, 0);
analogWrite(Backward4,100);
}
}
}