I want to control the throttle of my quadcopter using my HC-SR04 Ultrasonic Sensor & consist of KK 2.1.5 Flight controller, I successfully can arm and disarm the board by my sketch But unable to map the throttle with HC-SR04.
I want to map the range 0-50 Cm of HC-SR04 with bldc_Throttle.writeMicroseconds(1100) - bldc_Throttle.writeMicroseconds(2390)
that means if the range increase throttle should also increase
#include <Servo.h>
int 55;
int senseval;
#define echoPin 7
#define trigPin 8
#define LEDPin 13
int maximumRange = 200;
int minimumRange = 0;
long duration, distance;
Servo bldc_Aileron;
Servo bldc_Elevator;
Servo bldc_Throttle;
Servo bldc_Rudder;
Servo bldc_Aux;
Servo Aileron;
Servo Elevator;
Servo Throttle;
Servo Rudder;
Servo Aux;
int defAileron = 1500;
int defElevator = 1500;
int defThrottle = 1100;
int defRudder = 1500;
int deffThrottle = 2300;
int auxOn = 2018;
int auxOff = 1015;
void setup()
{
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
bldc_Aileron.attach(5);
bldc_Elevator.attach(6);
bldc_Throttle.attach(9);
bldc_Rudder.attach(10);
bldc_Aux.attach(11);
Serial.begin(9600);
delay(100);
}
void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = duration / 58.2;
Serial.println(distance);
delay (100);
senseval = constrain(distance, 0, 50);
if (Serial.available() > 0) {
int incomingByte = Serial.read();
if (incomingByte == 43) { // plus sign
bldc_Throttle.writeMicroseconds(defThrottle);
bldc_Rudder.writeMicroseconds(1100);
delay(2000);
bldc_Rudder.writeMicroseconds(defRudder);
Serial.println("Model Armed and Ready to Fly");
delay(1000);
}
else if (incomingByte == 45) { // minus sign
bldc_Throttle.writeMicroseconds(defThrottle);
bldc_Rudder.writeMicroseconds(1900);
delay(2000);
bldc_Rudder.writeMicroseconds(defRudder);
Serial.println("Model Disarmed");
delay(1000);
}
}
}
Thanks in advance