Servomotor.cpp

00001 #include<Servomotor.hpp>
00002 
00003 servo::servo(PinName signal,unsigned int range_degree,unsigned int pulse_cycle,unsigned int min_pulse,unsigned int max_pulse):signal_(signal),range_degree_(range_degree),pulse_cycle_(pulse_cycle),min_pulse_(min_pulse),max_pulse_(max_pulse)
00004 {
00005     now_angle_ = 0;
00006     pdp = (max_pulse - min_pulse)/range_degree;
00007 }
00008 
00009 void servo::Setangle(unsigned int tar_angle){
00010     PwmOut servo(signal_);
00011     servo.period_us(pulse_cycle_);
00012     unsigned int tar_angle_ = (tar_angle > range_degree_ ? range_degree_ :tar_angle);
00013     pulsese = min_pulse_+pdp*tar_angle_;
00014     servo.pulsewidth_us(pulsese);
00015     now_angle_ = tar_angle;
00016 }
00017 
00018 void servo::Addangle(int tar_angle){
00019     PwmOut servo(signal_);
00020     servo.period_us(pulse_cycle_);
00021     int tar_angle_ = now_angle_ + tar_angle;
00022     tar_angle_ = (tar_angle_ < 0 ? 0 : tar_angle_ > range_degree_ ? range_degree_ :tar_angle_);
00023     pulsese = min_pulse_+pdp*tar_angle_;
00024     servo.pulsewidth_us(pulsese);
00025     now_angle_ = tar_angle_;
00026 }