Matthias Grob
Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)
Diff: Servo/Servo.cpp
- Revision:
- 0:37f0c1e8fa66
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo/Servo.cpp Tue Sep 08 13:38:10 2015 +0000
@@ -0,0 +1,44 @@
+#include "Servo.h"
+#include "mbed.h"
+
+Servo::Servo(PinName Pin, int frequency) : ServoPin(Pin) {
+ Enable(1000,1000000/frequency); // low throttle 50Hz TODO: Frequency modify
+}
+
+void Servo::SetFrequency(int frequency) {
+ Pulse.detach();
+ Pulse.attach_us(this, &Servo::StartPulse, 1/frequency);
+}
+
+void Servo::SetPosition(int Pos) {
+ if (Pos > 1000)
+ Pos = 1000;
+ if (Pos < 0)
+ Pos = 0;
+ Position = Pos+1000;
+}
+
+void Servo::StartPulse() {
+ ServoPin = 1;
+ PulseStop.attach_us(this, &Servo::EndPulse, Position);
+}
+
+void Servo::EndPulse() {
+ // my change
+ PulseStop.detach();
+ // my change
+ ServoPin = 0;
+}
+
+void Servo::Enable(int StartPos, int Period) {
+ Position = StartPos;
+ Pulse.attach_us(this, &Servo::StartPulse, Period);
+}
+
+void Servo::Disable() {
+ Pulse.detach();
+}
+
+void Servo::operator=(int position) {
+ SetPosition(position);
+}
\ No newline at end of file