Created
June 20, 2019 02:38
-
-
Save dantiel/98d8f6027792aea59b0826ec1720f75c to your computer and use it in GitHub Desktop.
Arduino sketch for controlling ornitoptera using KST-320 servos
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//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
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
.