Nhi vtq / ServoMotorControl

Library is coded to control servo This library is part of my project which is built on FRDM-KL25Z and motor shield l293D v1

ServoMotorControl.cpp@0:22a971e060ed, 2017-09-24 (annotated)

Committer:
vtqNhi
Date:
Sun Sep 24 18:41:24 2017 +0000
Revision:
0:22a971e060ed
This library is created to control servo motor by send pulsewidth to pwm pin.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vtqNhi 0:22a971e060ed 1 #include "mbed.h"
vtqNhi 0:22a971e060ed 2 #include "ServoMotorControl.h"
vtqNhi 0:22a971e060ed 3
vtqNhi 0:22a971e060ed 4 static float clamp(float value, float min, float max) {
vtqNhi 0:22a971e060ed 5 if(value < min) {
vtqNhi 0:22a971e060ed 6 return min;
vtqNhi 0:22a971e060ed 7 } else if(value > max) {
vtqNhi 0:22a971e060ed 8 return max;
vtqNhi 0:22a971e060ed 9 } else {
vtqNhi 0:22a971e060ed 10 return value;
vtqNhi 0:22a971e060ed 11 }
vtqNhi 0:22a971e060ed 12 }
vtqNhi 0:22a971e060ed 13
vtqNhi 0:22a971e060ed 14
vtqNhi 0:22a971e060ed 15 Servo::Servo(PinName pwm) : _pwm(pwm)
vtqNhi 0:22a971e060ed 16 {
vtqNhi 0:22a971e060ed 17 _pwm = 0;
vtqNhi 0:22a971e060ed 18 }
vtqNhi 0:22a971e060ed 19
vtqNhi 0:22a971e060ed 20 void Servo::turn2Angle(int degree)
vtqNhi 0:22a971e060ed 21 {
vtqNhi 0:22a971e060ed 22 float angle2value;
vtqNhi 0:22a971e060ed 23 switch (degree){
vtqNhi 0:22a971e060ed 24 case 0: angle2value = 1; break;
vtqNhi 0:22a971e060ed 25 case 15: angle2value = 1.25; break;
vtqNhi 0:22a971e060ed 26 case 30: angle2value = 1.5; break;
vtqNhi 0:22a971e060ed 27 case 45: angle2value = 1.75; break;
vtqNhi 0:22a971e060ed 28 case 60: angle2value = 2; break;
vtqNhi 0:22a971e060ed 29 case 75: angle2value = 2.25; break;
vtqNhi 0:22a971e060ed 30 case 90: angle2value = 2.5; break;
vtqNhi 0:22a971e060ed 31 case 105: angle2value = 2.75; break;
vtqNhi 0:22a971e060ed 32 case 120: angle2value = 3; break;
vtqNhi 0:22a971e060ed 33 case 135: angle2value = 3.25; break;
vtqNhi 0:22a971e060ed 34 case 150: angle2value = 3.5; break;
vtqNhi 0:22a971e060ed 35 case 165: angle2value = 3.75; break;
vtqNhi 0:22a971e060ed 36 case 180: angle2value = 4; break;
vtqNhi 0:22a971e060ed 37 default: angle2value = 1; break;}
vtqNhi 0:22a971e060ed 38 turn2pulse(angle2value);
vtqNhi 0:22a971e060ed 39
vtqNhi 0:22a971e060ed 40 }
vtqNhi 0:22a971e060ed 41
vtqNhi 0:22a971e060ed 42 void Servo::turn2pulse(float value)
vtqNhi 0:22a971e060ed 43 {
vtqNhi 0:22a971e060ed 44 //0 1
vtqNhi 0:22a971e060ed 45 //30 1.5
vtqNhi 0:22a971e060ed 46 //60 2
vtqNhi 0:22a971e060ed 47 //90 2.5
vtqNhi 0:22a971e060ed 48 //120 3
vtqNhi 0:22a971e060ed 49 //150 3.5
vtqNhi 0:22a971e060ed 50 //180 4
vtqNhi 0:22a971e060ed 51 if (value < 1)
vtqNhi 0:22a971e060ed 52 {
vtqNhi 0:22a971e060ed 53 value = 1;
vtqNhi 0:22a971e060ed 54 }
vtqNhi 0:22a971e060ed 55 else if (value > 4)
vtqNhi 0:22a971e060ed 56 {
vtqNhi 0:22a971e060ed 57 value = 4;
vtqNhi 0:22a971e060ed 58 }
vtqNhi 0:22a971e060ed 59 value = value / 1500;
vtqNhi 0:22a971e060ed 60 _pwm.pulsewidth(value);
vtqNhi 0:22a971e060ed 61 }
vtqNhi 0:22a971e060ed 62 float Servo::positionRead()
vtqNhi 0:22a971e060ed 63 {
vtqNhi 0:22a971e060ed 64 return _position;
vtqNhi 0:22a971e060ed 65 }