Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/Servo.cpp

Committer:
hazheng
Date:
2017-03-30
Revision:
45:501b7909139a
Parent:
20:4cdbd1ae4ead
Child:
46:a5eb9bd3bb55

File content as of revision 45:501b7909139a:

#include "Servo.h"

#include "Core.h"
#include "PinAssignment.h"
#include "SWUSBServer.h"

#include "mbed.h"

#ifdef __cplusplus
extern "C" {
#endif

static float servo_pulseWidth = 0.0015;
static PwmOut servo_pwm(PIN_SC_SERVO);

void servo_init()
{
    servo_pwm.period(0.020);
    servo_pwm.pulsewidth(servo_pulseWidth);
}

void servo_set_angle(float angle)
{
    if(angle > SERVO_RT * SERVO_MAX_ANGLE)
        angle = SERVO_RT * SERVO_MAX_ANGLE;
    else if(angle < SERVO_LF * SERVO_MAX_ANGLE)
        angle = SERVO_LF * SERVO_MAX_ANGLE;
        
    servo_pulseWidth = (0.0015 + (0.00001667 * angle));
    //char buf[10];
    //sprintf(buf, "%f", m_pulseWidth);
    //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
    servo_pwm.pulsewidth(servo_pulseWidth); 
}

#ifdef __cplusplus
}
#endif
/*
Servo::Servo(SW::Core& core) :
    m_core(core),
    m_pwm(PwmOut(PIN_SC_SERVO))
    
{
     m_pulseWidth = 0.0015;
     m_pwm.period(0.020);
     m_pwm.pulsewidth(m_pulseWidth);
}


void Servo::Update(float deltaTime)
{
    
}

void Servo::setAngle(float angle)
{
    if(angle > SERVO_RT * SERVO_MAX_ANGLE)
        angle = SERVO_RT * SERVO_MAX_ANGLE;
    else if(angle < SERVO_LF * SERVO_MAX_ANGLE)
        angle = SERVO_LF * SERVO_MAX_ANGLE;
        
    m_pulseWidth = (0.0015 + (0.00001667 * angle));
    //char buf[10];
    //sprintf(buf, "%f", m_pulseWidth);
    //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
    m_pwm.pulsewidth(m_pulseWidth); 
}
*/