Diff: ServoMotor.cpp

Revision:

1:66011300b267

Parent:

0:908ddd5c615d

Child:

3:5fb516a5f593

--- a/ServoMotor.cpp	Fri Oct 18 16:18:07 2019 +0000
+++ b/ServoMotor.cpp	Thu Oct 24 05:20:08 2019 +0000
@@ -1,35 +1,51 @@
 #include "mbed.h"
 #include "ServoMotor.h"
 
-ServoMotor_270::ServoMotor_270(PinName servo_pwmport_,float init_deg):servo_pwmport(servo_pwmport_){
-    servo_pwmport.period(0.02);         //pwm周期を20msにする
-    output_deg = init_deg;           //outputの初期化
-    servo_pwmport.pulsewidth(0.0007);
-    servo_pwmport.pulsewidth((init_deg/168750.0)+0.0007);
-}
 
-void ServoMotor_270::rot(float input_deg){
-    
-    float temp = 0.0;
-    
-    if(input_deg<0.0){                  //0度以下の値は0にする
-        output_deg=0.0;
-    }
-    else if(input_deg>270.0){
-        output_deg=270.0;
-    }
-    
-    output_deg=input_deg; 
-    servo_pwmport.pulsewidth((output_deg/168750.0)+0.0007);//pwmのパルスの出力幅を計算。この計算は23msが270度、7msが0度の計算式。
+/* ServoMotorコンストラクタ */
+ServoMotor::ServoMotor( PinName servo_pwmport_,
+                        float   minDeg_,
+                        float   maxDeg_,
+                        float   pulse_width_,
+                        float   minDPLW_,
+                        float   maxDPLW_
+                      ):servo_pwmport(servo_pwmport_)
+{
+
+    minDeg = minDeg_;
+    maxDeg = maxDeg_;
+    pulse_width = pulse_width_;
+    minDPLW = minDPLW_;
+    maxDPLW = maxDPLW_;
+
+    servo_pwmport.period(pulse_width);                  //pwm周期を20msにする
+
+    decomposition_value = maxDeg / (maxDPLW - minDPLW); //分解値の計算
 }
 
 
-float ServoMotor_270::read(){
-    return output_deg;
+
+/* モータを回転させる関数 引数:目標角度 */
+void ServoMotor::rot(float target_deg_)
+{
+    if(target_deg_ == output_deg_b){
+        ;
+    }else{
+        if(target_deg_< minDeg)         //minDeg度より小さい値はminDegにする
+            target_deg_ = minDeg;
+        else if(target_deg_ > maxDeg)   //maxDeg度より大きい値はmaxDegにする
+            target_deg_ = maxDeg;
+    
+        output_deg = target_deg_;
+        servo_pwmport.pulsewidth((output_deg / decomposition_value) + minDPLW); //pwmのパルスの出力幅を計算。この計算はmaxDPLWがmaxDeg,minDPLWがminDegになるような計算式.
+        
+        output_deg_b = output_deg;
+    }
 }
 
 
-void ServoMotor_270::weak_condition(){
-       servo_pwmport.pulsewidth(0.00005);   //脱力状態(50usで発生する)
+void ServoMotor::weak_condition()
+{
+    servo_pwmport.pulsewidth(0.00005);   //脱力状態(50usで発生する)
 }