_a i_ / ServoMotor

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

Dependents:   2020_lab_No2reactor_contorl_ver2

ServoMotor.cpp@4:314817c5ec80, 2020-07-29 (annotated)

Committer:
_ai_
Date:
Wed Jul 29 23:30:34 2020 +0000
Revision:
4:314817c5ec80
Parent:
3:5fb516a5f593
Child:
5:d3104455fed9
change input =0.00001 when input = 0 .; ;

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_ 4:314817c5ec80 31 if(target_deg_=0.0){
_ai_ 4:314817c5ec80 32 target_deg_ = 0.00001;
_ai_ 4:314817c5ec80 33 }
_ai_ 4:314817c5ec80 34
_ai_ 3:5fb516a5f593 35 if(target_deg_ == output_deg_b){ //もし前回の値と同じ値が続いたときは電圧の出力はしないためのフィルタ
_ai_ 1:66011300b267 36 ;
_ai_ 1:66011300b267 37 }else{
_ai_ 1:66011300b267 38 if(target_deg_< minDeg) //minDeg度より小さい値はminDegにする
_ai_ 1:66011300b267 39 target_deg_ = minDeg;
_ai_ 1:66011300b267 40 else if(target_deg_ > maxDeg) //maxDeg度より大きい値はmaxDegにする
_ai_ 1:66011300b267 41 target_deg_ = maxDeg;
_ai_ 1:66011300b267 42
_ai_ 1:66011300b267 43 output_deg = target_deg_;
_ai_ 1:66011300b267 44 servo_pwmport.pulsewidth((output_deg / decomposition_value) + minDPLW); //pwmのパルスの出力幅を計算。この計算はmaxDPLWがmaxDeg,minDPLWがminDegになるような計算式.
_ai_ 1:66011300b267 45
_ai_ 1:66011300b267 46 output_deg_b = output_deg;
_ai_ 1:66011300b267 47 }
_ai_ 0:908ddd5c615d 48 }
_ai_ 0:908ddd5c615d 49
_ai_ 0:908ddd5c615d 50
_ai_ 1:66011300b267 51 void ServoMotor::weak_condition()
_ai_ 1:66011300b267 52 {
_ai_ 1:66011300b267 53 servo_pwmport.pulsewidth(0.00005); //脱力状態(50usで発生する)
_ai_ 0:908ddd5c615d 54 }
_ai_ 0:908ddd5c615d 55