Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
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
You can’t perform that action at this time.