Arduino sketch for controlling ornitoptera using KST-320 servos
//Arduino servo flap system | |
//copyright Steve Morris 10-25-16 | |
#include <DigitalServo.h> | |
#include <PPMReader.h> | |
#include <InterruptHandler.h> | |
int interruptPin = 2; | |
int channelAmount = 6; | |
PPMReader ppm(interruptPin, channelAmount); | |
int servo_left_pin = 4; | |
int servo_right_pin = 6; | |
float elevator = 0; | |
volatile int ppm1_value = 1000; | |
volatile int ppm2_value = 1550; | |
volatile int ppm3_value = 1500; | |
volatile int ppm5_value = 1500; | |
volatile int prev_timeD2 = 0; | |
volatile int prev_timeD3 = 0; | |
static int servo_comm1 = 0; | |
static int servo_comm2 = 0; | |
volatile int flapangle1 = 0; | |
volatile int flapangle2 = 0; | |
volatile int rudder = 0; | |
float rudderamp = 0; | |
volatile int millisold = 0; | |
int millinow = 0; | |
float dt = 0; | |
float flapmag = 0; | |
static float flapdeg = 0; | |
float tcommand = 0; | |
float floattime = 0; | |
float glide_deg = 6.0; | |
float omegadot = 0.0; | |
float thetadot = 0.0; | |
static float omega = 0.0; | |
static float theta = 0.0; | |
static float k0 = 1.0; | |
static float k2 = 10.0; | |
static float servo_zero1 = 0; | |
static float servo_zero2 = 0; | |
DigitalServo servo_left, servo_right; // create servo object to control a servo | |
void setup() { | |
// Serial.begin(9600); | |
pinMode(servo_left_pin, OUTPUT); | |
pinMode(servo_right_pin, OUTPUT); | |
servo_left.attach(servo_left_pin); | |
servo_right.attach(servo_right_pin); | |
} | |
void loop() { | |
ppm1_value = ppm.rawChannelValue(3); | |
ppm2_value = ppm.rawChannelValue(1); | |
ppm3_value = ppm.rawChannelValue(2); | |
ppm5_value = ppm.rawChannelValue(5); | |
millinow = millis(); | |
floattime = millinow / 1000.0; | |
dt = (millinow - millisold) / 1000.0; | |
millisold = millinow; | |
tcommand = (ppm1_value - 480.0) / 8.22; | |
omegadot = k0*tcommand - k2*omega; | |
thetadot = omega; | |
flapdeg = sin(theta); | |
theta = theta + omega*dt; | |
omega = omega + omegadot*dt; | |
// ppm1_value = 2000 | |
// (ppm1_value - 1000) / 25.0 + 6 | |
flapmag = (ppm1_value - 1055) / 45.0 + 2; | |
flapdeg = flapmag * sin(theta); //variable amplitude+freq | |
rudderamp = ((1548.0 / ppm2_value) - 1) / 8 + 1; | |
rudder = (int)((ppm2_value - 1540) / 50); | |
elevator = (ppm3_value / 33 - 45) * (ppm5_value / 700); | |
flapangle1 = (int)((rudder - (flapdeg / rudderamp) + servo_zero1 - elevator) * 2.0); | |
flapangle2 = (int)((rudder + (flapdeg * rudderamp) + servo_zero2 + elevator) * 2.0); | |
if (ppm1_value > 1040) { | |
servo_comm1 = flapangle1; | |
servo_comm2 = flapangle2; | |
} | |
// Glide Lock | |
if (ppm1_value < 1030) { | |
servo_comm1 = (int)((rudder - glide_deg + servo_zero1 - elevator) * 2.0); | |
servo_comm2 = (int)((rudder + glide_deg + servo_zero2 + elevator) * 2.0); | |
} | |
// Serial.println(ppm1_value); | |
// Serial.print(servo_comm1); | |
// Serial.print(", "); | |
// Serial.print(servo_comm2); | |
// Serial.print(", "); | |
servo_comm1 = servo_comm1 + 100; | |
servo_comm2 = servo_comm2 + 100; | |
servo_left.write(servo_comm1); // tell servo to go to position in variable 'pos' | |
servo_right.write(servo_comm2); // tell servo to go to position in variable 'pos' | |
// | |
// Serial.println(ppm1_value); | |
// Serial.print(", "); | |
// Serial.print(ppm2_value); | |
// Serial.print(", "); | |
// // Serial.print(1548.0 / ppm2_value); | |
// // Serial.print(", "); | |
// Serial.print(rudderamp); | |
// Serial.println(rudderamp); | |
// Serial.print(", "); | |
// Serial.print(elevator); | |
// Serial.print(", "); | |
// Serial.print(servo_comm1); | |
// Serial.print(", "); | |
// Serial.print(servo_comm2); | |
// // Serial.println(); | |
// | |
// Serial.print(flapdeg); | |
// Serial.print(", "); | |
// Serial.print(floattime); | |
// Serial.print(", "); | |
// Serial.print(dt); | |
// | |
// Serial.print(", "); | |
// Serial.print(flapangle2); | |
// Serial.print(", "); | |
// Serial.println("); | |
// Serial.print(tcommand); | |
// Serial.print(", "); | |
// Serial.println(ppm1_value); | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
This comment has been minimized.
For this implementation a PPM receiver is being used hence we need an additional library to read the different channel values. Place code from https://github.com/Nikkilae/PPM-reader into
~/Documents/Arduino/libraries
.