Diff: actuator/Servomotor.cpp

Revision:

2:e7b09385d197

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actuator/Servomotor.cpp	Sun Mar 27 04:39:16 2022 +0000
@@ -0,0 +1,26 @@
+#include<Servomotor.hpp>
+
+servo::servo(PinName signal,unsigned int range_degree,unsigned int pulse_cycle,unsigned int min_pulse,unsigned int max_pulse):signal_(signal),range_degree_(range_degree),pulse_cycle_(pulse_cycle),min_pulse_(min_pulse),max_pulse_(max_pulse)
+{
+    now_angle_ = 0;
+    pdp = (max_pulse - min_pulse)/range_degree;
+}
+
+void servo::Setangle(unsigned int tar_angle){
+    PwmOut servo(signal_);
+    servo.period_us(pulse_cycle_);
+    unsigned int tar_angle_ = (tar_angle > range_degree_ ? range_degree_ :tar_angle);
+    pulsese = min_pulse_+pdp*tar_angle_;
+    servo.pulsewidth_us(pulsese);
+    now_angle_ = tar_angle;
+}
+
+void servo::Addangle(int tar_angle){
+    PwmOut servo(signal_);
+    servo.period_us(pulse_cycle_);
+    int tar_angle_ = now_angle_ + tar_angle;
+    tar_angle_ = (tar_angle_ < 0 ? 0 : tar_angle_ > range_degree_ ? range_degree_ :tar_angle_);
+    pulsese = min_pulse_+pdp*tar_angle_;
+    servo.pulsewidth_us(pulsese);
+    now_angle_ = tar_angle_;
+}
\ No newline at end of file