Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Servo/Servo.cpp@5:06e978fd147a, 2014-07-06 (annotated)
- Committer:
- maetugr
- Date:
- Sun Jul 06 05:51:25 2014 +0000
- Revision:
- 5:06e978fd147a
switched from dedicated PWM Motor Driver to Interrupt Motor Driver which works on every Pin because PWM crashed a lot of I2C Sensor Values!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 5:06e978fd147a | 1 | #include "Servo.h" |
maetugr | 5:06e978fd147a | 2 | #include "mbed.h" |
maetugr | 5:06e978fd147a | 3 | |
maetugr | 5:06e978fd147a | 4 | Servo::Servo(PinName Pin, int frequency) : ServoPin(Pin) { |
maetugr | 5:06e978fd147a | 5 | Enable(1000,1000000/frequency); // low throttle 50Hz TODO: Frequency modify |
maetugr | 5:06e978fd147a | 6 | } |
maetugr | 5:06e978fd147a | 7 | |
maetugr | 5:06e978fd147a | 8 | void Servo::SetFrequency(int frequency) { |
maetugr | 5:06e978fd147a | 9 | Pulse.detach(); |
maetugr | 5:06e978fd147a | 10 | Pulse.attach_us(this, &Servo::StartPulse, 1/frequency); |
maetugr | 5:06e978fd147a | 11 | } |
maetugr | 5:06e978fd147a | 12 | |
maetugr | 5:06e978fd147a | 13 | void Servo::SetPosition(int Pos) { |
maetugr | 5:06e978fd147a | 14 | if (Pos > 1000) |
maetugr | 5:06e978fd147a | 15 | Pos = 1000; |
maetugr | 5:06e978fd147a | 16 | if (Pos < 0) |
maetugr | 5:06e978fd147a | 17 | Pos = 0; |
maetugr | 5:06e978fd147a | 18 | Position = Pos+1000; |
maetugr | 5:06e978fd147a | 19 | } |
maetugr | 5:06e978fd147a | 20 | |
maetugr | 5:06e978fd147a | 21 | void Servo::StartPulse() { |
maetugr | 5:06e978fd147a | 22 | ServoPin = 1; |
maetugr | 5:06e978fd147a | 23 | PulseStop.attach_us(this, &Servo::EndPulse, Position); |
maetugr | 5:06e978fd147a | 24 | } |
maetugr | 5:06e978fd147a | 25 | |
maetugr | 5:06e978fd147a | 26 | void Servo::EndPulse() { |
maetugr | 5:06e978fd147a | 27 | // my change |
maetugr | 5:06e978fd147a | 28 | PulseStop.detach(); |
maetugr | 5:06e978fd147a | 29 | // my change |
maetugr | 5:06e978fd147a | 30 | ServoPin = 0; |
maetugr | 5:06e978fd147a | 31 | } |
maetugr | 5:06e978fd147a | 32 | |
maetugr | 5:06e978fd147a | 33 | void Servo::Enable(int StartPos, int Period) { |
maetugr | 5:06e978fd147a | 34 | Position = StartPos; |
maetugr | 5:06e978fd147a | 35 | Pulse.attach_us(this, &Servo::StartPulse, Period); |
maetugr | 5:06e978fd147a | 36 | } |
maetugr | 5:06e978fd147a | 37 | |
maetugr | 5:06e978fd147a | 38 | void Servo::Disable() { |
maetugr | 5:06e978fd147a | 39 | Pulse.detach(); |
maetugr | 5:06e978fd147a | 40 | } |
maetugr | 5:06e978fd147a | 41 | |
maetugr | 5:06e978fd147a | 42 | void Servo::operator=(int position) { |
maetugr | 5:06e978fd147a | 43 | SetPosition(position); |
maetugr | 5:06e978fd147a | 44 | } |