Daisuke Nakayama / ServoMotor

library for RC servo motor

Dependents:   yagi_Rotator_Ver2

ServoMotor.cpp@0:b46b05ed2f27, 2020-03-08 (annotated)

Committer:
j_rocket_boy
Date:
Sun Mar 08 03:39:10 2020 +0000
Revision:
0:b46b05ed2f27
for Antenna rotator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_rocket_boy 0:b46b05ed2f27 1 #include "ServoMotor.h"
j_rocket_boy 0:b46b05ed2f27 2 #include "mbed.h"
j_rocket_boy 0:b46b05ed2f27 3
j_rocket_boy 0:b46b05ed2f27 4 SERVO_MOTOR :: SERVO_MOTOR(PinName pin,
j_rocket_boy 0:b46b05ed2f27 5 double angle0, double PulseWidth0_us,
j_rocket_boy 0:b46b05ed2f27 6 double angle1, double PulseWidth1_us) : _pwm(pin) {
j_rocket_boy 0:b46b05ed2f27 7
j_rocket_boy 0:b46b05ed2f27 8 if(angle0 < angle1){
j_rocket_boy 0:b46b05ed2f27 9 _angle0 = angle0;
j_rocket_boy 0:b46b05ed2f27 10 _angle1 = angle1;
j_rocket_boy 0:b46b05ed2f27 11 _PulseWidth0_us = PulseWidth0_us;
j_rocket_boy 0:b46b05ed2f27 12 _PulseWidth1_us = PulseWidth1_us;
j_rocket_boy 0:b46b05ed2f27 13 }else{
j_rocket_boy 0:b46b05ed2f27 14 _angle0 = angle1;
j_rocket_boy 0:b46b05ed2f27 15 _angle1 = angle0;
j_rocket_boy 0:b46b05ed2f27 16 _PulseWidth0_us = PulseWidth1_us;
j_rocket_boy 0:b46b05ed2f27 17 _PulseWidth1_us = PulseWidth0_us;
j_rocket_boy 0:b46b05ed2f27 18 }
j_rocket_boy 0:b46b05ed2f27 19 _pwm.period_ms(20);// pulse cycle = 20ms
j_rocket_boy 0:b46b05ed2f27 20 _speed = 10; //5deg/s
j_rocket_boy 0:b46b05ed2f27 21 _timer.start();
j_rocket_boy 0:b46b05ed2f27 22 if(_angle0 <= 0 && 0 <= _angle1){
j_rocket_boy 0:b46b05ed2f27 23 _angle_now = 0;
j_rocket_boy 0:b46b05ed2f27 24 }else{
j_rocket_boy 0:b46b05ed2f27 25 _angle_now = (angle0+angle1)/2.0;
j_rocket_boy 0:b46b05ed2f27 26 }
j_rocket_boy 0:b46b05ed2f27 27
j_rocket_boy 0:b46b05ed2f27 28 }
j_rocket_boy 0:b46b05ed2f27 29
j_rocket_boy 0:b46b05ed2f27 30 void SERVO_MOTOR :: set_speed(double speed){
j_rocket_boy 0:b46b05ed2f27 31 _speed = speed;
j_rocket_boy 0:b46b05ed2f27 32 }
j_rocket_boy 0:b46b05ed2f27 33
j_rocket_boy 0:b46b05ed2f27 34 double SERVO_MOTOR :: get_angle(void){
j_rocket_boy 0:b46b05ed2f27 35 return _angle_now;
j_rocket_boy 0:b46b05ed2f27 36 }
j_rocket_boy 0:b46b05ed2f27 37
j_rocket_boy 0:b46b05ed2f27 38 bool SERVO_MOTOR :: tick(void){
j_rocket_boy 0:b46b05ed2f27 39
j_rocket_boy 0:b46b05ed2f27 40 double PulseWidth_us;
j_rocket_boy 0:b46b05ed2f27 41 double time_now;
j_rocket_boy 0:b46b05ed2f27 42
j_rocket_boy 0:b46b05ed2f27 43 if(_time > _timer.read()){
j_rocket_boy 0:b46b05ed2f27 44 return true;
j_rocket_boy 0:b46b05ed2f27 45 }
j_rocket_boy 0:b46b05ed2f27 46
j_rocket_boy 0:b46b05ed2f27 47 if(_angle_now == _angle_command){
j_rocket_boy 0:b46b05ed2f27 48 return false;
j_rocket_boy 0:b46b05ed2f27 49 }
j_rocket_boy 0:b46b05ed2f27 50
j_rocket_boy 0:b46b05ed2f27 51 time_now = _timer.read();
j_rocket_boy 0:b46b05ed2f27 52
j_rocket_boy 0:b46b05ed2f27 53 if(_angle_command > _angle_now){
j_rocket_boy 0:b46b05ed2f27 54 if(_angle_command > _angle_now + _speed*(time_now - _time)){
j_rocket_boy 0:b46b05ed2f27 55 _angle_now += _speed*(time_now - _time);
j_rocket_boy 0:b46b05ed2f27 56 }else{
j_rocket_boy 0:b46b05ed2f27 57 _angle_now = _angle_command;
j_rocket_boy 0:b46b05ed2f27 58 }
j_rocket_boy 0:b46b05ed2f27 59 }else{
j_rocket_boy 0:b46b05ed2f27 60 if(_angle_command < _angle_now - _speed*(time_now - _time)){
j_rocket_boy 0:b46b05ed2f27 61 _angle_now -= _speed*(time_now - _time);
j_rocket_boy 0:b46b05ed2f27 62 }else{
j_rocket_boy 0:b46b05ed2f27 63 _angle_now = _angle_command;
j_rocket_boy 0:b46b05ed2f27 64 }
j_rocket_boy 0:b46b05ed2f27 65 }
j_rocket_boy 0:b46b05ed2f27 66
j_rocket_boy 0:b46b05ed2f27 67 _time = time_now;
j_rocket_boy 0:b46b05ed2f27 68
j_rocket_boy 0:b46b05ed2f27 69 PulseWidth_us = (_PulseWidth1_us - _PulseWidth0_us)/(_angle1 - _angle0) * (_angle_now - _angle0) + _PulseWidth0_us;
j_rocket_boy 0:b46b05ed2f27 70 _pwm.pulsewidth(PulseWidth_us * 1e-6);
j_rocket_boy 0:b46b05ed2f27 71
j_rocket_boy 0:b46b05ed2f27 72 return true;
j_rocket_boy 0:b46b05ed2f27 73
j_rocket_boy 0:b46b05ed2f27 74 }
j_rocket_boy 0:b46b05ed2f27 75
j_rocket_boy 0:b46b05ed2f27 76 void SERVO_MOTOR :: move(double angle, bool sync){
j_rocket_boy 0:b46b05ed2f27 77
j_rocket_boy 0:b46b05ed2f27 78 if(angle < _angle0){
j_rocket_boy 0:b46b05ed2f27 79 angle = _angle0;
j_rocket_boy 0:b46b05ed2f27 80 }
j_rocket_boy 0:b46b05ed2f27 81 if(angle > _angle1){
j_rocket_boy 0:b46b05ed2f27 82 angle = _angle1;
j_rocket_boy 0:b46b05ed2f27 83 }
j_rocket_boy 0:b46b05ed2f27 84
j_rocket_boy 0:b46b05ed2f27 85 _angle_command = angle;
j_rocket_boy 0:b46b05ed2f27 86 _timer.reset();
j_rocket_boy 0:b46b05ed2f27 87 _time = _timer.read();
j_rocket_boy 0:b46b05ed2f27 88
j_rocket_boy 0:b46b05ed2f27 89 if(sync){
j_rocket_boy 0:b46b05ed2f27 90 while(tick());
j_rocket_boy 0:b46b05ed2f27 91 }
j_rocket_boy 0:b46b05ed2f27 92
j_rocket_boy 0:b46b05ed2f27 93 }
j_rocket_boy 0:b46b05ed2f27 94
j_rocket_boy 0:b46b05ed2f27 95 void SERVO_MOTOR :: go(double angle){
j_rocket_boy 0:b46b05ed2f27 96 double PulseWidth_us;
j_rocket_boy 0:b46b05ed2f27 97 _angle_command = angle;
j_rocket_boy 0:b46b05ed2f27 98 _angle_now = _angle_command;
j_rocket_boy 0:b46b05ed2f27 99 PulseWidth_us = (_PulseWidth1_us - _PulseWidth0_us)/(_angle1 - _angle0) * (_angle_now - _angle0) + _PulseWidth0_us;
j_rocket_boy 0:b46b05ed2f27 100 _pwm.pulsewidth(PulseWidth_us * 1e-6);
j_rocket_boy 0:b46b05ed2f27 101 }