NerfUS
Servomotor
Dependents: ServomotorTests NerfUSTarget
Diff: Servomotor.cpp
- Revision:
- 0:46b4e7d76586
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Servomotor.cpp Sat Feb 25 22:57:27 2017 +0000
@@ -0,0 +1,26 @@
+#include "Servomotor.hpp"
+
+Servomotor::Servomotor(PwmOutInterface &pwm_out) :
+ pwm_out(pwm_out)
+{
+
+}
+
+//Angle 0 degree = duty cycle 1 ms
+//Angle 90 degree = duty cycle 1.5 ms
+//Angle 180 degree = duty cycle 2 ms
+void Servomotor::set_angle(const float angle_degree)
+{
+ const float duty_cycle_ms = angle_degree / 180.0 + 1.0;
+ pwm_out.pulsewidth_ms(duty_cycle_ms);
+}
+
+void Servomotor::set_position_up()
+{
+ set_angle(0);
+}
+
+void Servomotor::set_position_down()
+{
+ set_angle(90);
+}
\ No newline at end of file