Hello everyone,
I'm trying to make an RC airplane with a Teensy 3.2 which reads PWM signals from the receiver and outputs them to servos. However, even with a fast processor, the servo output is still very slow and jagged. I know that the pulseIn function, which reads the PWM signal slows it down, and most likely the servo library. I am wondering if:
- There is a simple way to read PWM signals without slowing the whole loop down
- There is a simple way to output servo signals to servos without slowing the program down
- There are any other parts of code that might cause everything to slow down.
Thank you so much,
Kevin
P.S does the Arduino.h or Wire.h library do anything that would lag the program?
//Libraries
#include <Arduino.h>
#include <TinyMPU6050.h>
#include <Wire.h>
#include <Servo.h>
//setup connection to MPU6050
MPU6050 mpu (Wire);
//servos
Servo LeftElevon;
Servo RightElevon;
Servo ESC;
//trim
int leftTrim = 0;
int rightTrim = 0;
int throttle; //rx input left stick y axis
int rollIn; //rx input right stickj x axis
int pitchIn; //rx input right stick y axis
int mode; //rx input for switch
int reOut;
int leOut;
int escOut;
void setup() {
Serial.begin(9600); //serial output for debugging
mpu.Initialize();//initalize the mpu6050
//assign the outputs to pins
ESC.attach(23, 1000, 2000); //servo 1
LeftElevon.attach(22, 1000, 2000); //servo 2
RightElevon.attach(21, 1000, 2000); //servo 3
pinMode(7, OUTPUT); //LED pin
pinMode(A2, INPUT); //arispeed
//for the rx input
pinMode(10, INPUT);
pinMode(3, INPUT);
pinMode(4, INPUT);
pinMode(5, INPUT);
//arm the ESC
ESC.write(40);
delay(1000);
ESC.write(0);
//startup LED 2 blinks
digitalWrite(7, HIGH);
delay(1000);
digitalWrite(7, LOW);
delay(1000);
digitalWrite(7, HIGH);
delay(1000);
digitalWrite(7, LOW);
}
void loop() {
RxDetect(); //detect pilot commands
ServoOutput();
}
void RxDetect() {
//pilot command
throttle = pulseIn(10, HIGH, 25000); //channel 1
throttle = map(throttle, 1100, 1900, 0, 180);
rollIn = pulseIn(4, HIGH, 25000);
rollIn = map(rollIn, 1100, 1900, 0, 180);
pitchIn = pulseIn(5, HIGH, 25000);
pitchIn = map(pitchIn, 1100, 1900, 0, 180);
mode = pulseIn(3, HIGH, 25000); //channel 4
if (mode < 1500) {
mode = 1;
}
else if (mode > 1500) {
mode = 2;
}
}
void ServoOutput() {
leOut; //left elevon output
reOut; //right elevon output
escOut = throttle; //ESC output
//mixing this one is the one where roll plus pitch is equal to full actuation
leOut = (rollIn / 2) + (pitchIn / 2) + leftTrim;
reOut = (rollIn / 2) + ((180 - pitchIn) / 2) + rightTrim;
//mixing this one is one axis maxes the servos out
leOut = (rollIn - 45) + (pitchIn - 45) + leftTrim;
reOut = (rollIn - 45) + (135 - pitchIn) + rightTrim;
leOut = constrain(leOut, 0, 180);
reOut = constrain(reOut, 0, 180);
Serial.print(leOut);
Serial.print(",");
Serial.println(reOut);
LeftElevon.write(leOut);
RightElevon.write(reOut);
ESC.write(escOut);
}