ServoMotorControl.cpp

00001 #include "mbed.h"
00002 #include "ServoMotorControl.h"
00003 
00004 static float clamp(float value, float min, float max) {
00005     if(value < min) {
00006         return min;
00007     } else if(value > max) {
00008         return max;
00009     } else {
00010         return value;
00011     }
00012 }
00013 
00014 
00015 Servo::Servo(PinName pwm) : _pwm(pwm)
00016 {
00017     _pwm = 0;
00018 }
00019 
00020 void Servo::turn2Angle(int degree)
00021 {
00022     float angle2value;
00023     switch (degree){
00024     case 0: angle2value = 1; break;
00025     case 15: angle2value = 1.25; break;
00026     case 30: angle2value = 1.5; break;
00027     case 45: angle2value = 1.75; break;
00028     case 60: angle2value = 2; break;
00029     case 75: angle2value = 2.25; break;
00030     case 90: angle2value = 2.5; break;
00031     case 105: angle2value = 2.75; break;
00032     case 120: angle2value = 3; break;
00033     case 135: angle2value = 3.25; break;
00034     case 150: angle2value = 3.5; break;
00035     case 165: angle2value = 3.75; break;
00036     case 180: angle2value = 4; break;
00037     default: angle2value = 1; break;}
00038     turn2pulse(angle2value);
00039     
00040 }
00041 
00042 void Servo::turn2pulse(float value)
00043 {
00044     //0     1
00045     //30    1.5
00046     //60    2
00047     //90    2.5
00048     //120   3
00049     //150   3.5
00050     //180   4
00051     if (value < 1)
00052     {
00053         value = 1;
00054     }
00055     else if (value > 4)
00056     {
00057         value = 4;
00058     }
00059     value = value / 1500;
00060     _pwm.pulsewidth(value);
00061 }
00062 float Servo::positionRead()
00063 {
00064     return _position;
00065 }