NerfUS / Servomotor

Servomotor

Dependents:   ServomotorTests NerfUSTarget

source/Servomotor.cpp

Committer:
GaiSensei
Date:
2017-04-11
Revision:
7:a5b6e6d9ad3d
Parent:
6:8d8268e5a561
Child:
8:e42a3a06e072

File content as of revision 7:a5b6e6d9ad3d:

#include "Servomotor.hpp"

Servomotor::Servomotor(PwmOutInterface &pwm_out) : 
    pwm_out(pwm_out),
    up_angle(90)
{
    
}

//Angle 0 degree = duty cycle 500 ns
//Angle 90 degree = duty cycle 1500 ns
//Angle 180 degree = duty cycle 2500 ns
void Servomotor::set_angle(const float angle_degree)
{
    const float duty_cycle_us = (angle_degree / 180.0) * 2000 + 500;
    pwm_out.pulsewidth_us(duty_cycle_us);
}

void Servomotor::set_position_up()
{
    set_angle(up_angle);
}

void Servomotor::set_position_down()
{
    set_angle(up_angle - 75);
}