_a i_ / ServoMotor

サーボモータをPWM制御で動作させる関数

Dependents:   2020_lab_No2reactor_contorl_ver2

ServoMotor.cpp@1:66011300b267, 2019-10-24 (annotated)

Committer:
_ai_
Date:
Thu Oct 24 05:20:08 2019 +0000
Revision:
1:66011300b267
Parent:
0:908ddd5c615d
Child:
3:5fb516a5f593
Add a variable output_deg_b to filter.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_ai_ 0:908ddd5c615d 1 #include "mbed.h"
_ai_ 0:908ddd5c615d 2 #include "ServoMotor.h"
_ai_ 0:908ddd5c615d 3
_ai_ 0:908ddd5c615d 4
_ai_ 1:66011300b267 5 /* ServoMotorコンストラクタ */
_ai_ 1:66011300b267 6 ServoMotor::ServoMotor( PinName servo_pwmport_,
_ai_ 1:66011300b267 7 float minDeg_,
_ai_ 1:66011300b267 8 float maxDeg_,
_ai_ 1:66011300b267 9 float pulse_width_,
_ai_ 1:66011300b267 10 float minDPLW_,
_ai_ 1:66011300b267 11 float maxDPLW_
_ai_ 1:66011300b267 12 ):servo_pwmport(servo_pwmport_)
_ai_ 1:66011300b267 13 {
_ai_ 1:66011300b267 14
_ai_ 1:66011300b267 15 minDeg = minDeg_;
_ai_ 1:66011300b267 16 maxDeg = maxDeg_;
_ai_ 1:66011300b267 17 pulse_width = pulse_width_;
_ai_ 1:66011300b267 18 minDPLW = minDPLW_;
_ai_ 1:66011300b267 19 maxDPLW = maxDPLW_;
_ai_ 1:66011300b267 20
_ai_ 1:66011300b267 21 servo_pwmport.period(pulse_width); //pwm周期を20msにする
_ai_ 1:66011300b267 22
_ai_ 1:66011300b267 23 decomposition_value = maxDeg / (maxDPLW - minDPLW); //分解値の計算
_ai_ 0:908ddd5c615d 24 }
_ai_ 0:908ddd5c615d 25
_ai_ 0:908ddd5c615d 26
_ai_ 1:66011300b267 27
_ai_ 1:66011300b267 28 /* モータを回転させる関数 引数:目標角度 */
_ai_ 1:66011300b267 29 void ServoMotor::rot(float target_deg_)
_ai_ 1:66011300b267 30 {
_ai_ 1:66011300b267 31 if(target_deg_ == output_deg_b){
_ai_ 1:66011300b267 32 ;
_ai_ 1:66011300b267 33 }else{
_ai_ 1:66011300b267 34 if(target_deg_< minDeg) //minDeg度より小さい値はminDegにする
_ai_ 1:66011300b267 35 target_deg_ = minDeg;
_ai_ 1:66011300b267 36 else if(target_deg_ > maxDeg) //maxDeg度より大きい値はmaxDegにする
_ai_ 1:66011300b267 37 target_deg_ = maxDeg;
_ai_ 1:66011300b267 38
_ai_ 1:66011300b267 39 output_deg = target_deg_;
_ai_ 1:66011300b267 40 servo_pwmport.pulsewidth((output_deg / decomposition_value) + minDPLW); //pwmのパルスの出力幅を計算。この計算はmaxDPLWがmaxDeg,minDPLWがminDegになるような計算式.
_ai_ 1:66011300b267 41
_ai_ 1:66011300b267 42 output_deg_b = output_deg;
_ai_ 1:66011300b267 43 }
_ai_ 0:908ddd5c615d 44 }
_ai_ 0:908ddd5c615d 45
_ai_ 0:908ddd5c615d 46
_ai_ 1:66011300b267 47 void ServoMotor::weak_condition()
_ai_ 1:66011300b267 48 {
_ai_ 1:66011300b267 49 servo_pwmport.pulsewidth(0.00005); //脱力状態(50usで発生する)
_ai_ 0:908ddd5c615d 50 }
_ai_ 0:908ddd5c615d 51