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:
Bobymicjohn
Date:
2017-02-09
Revision:
14:88302da8bff0
Parent:
13:7dcb1642ef99
Child:
20:4cdbd1ae4ead

File content as of revision 14:88302da8bff0:

#include "Servo.h"

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

#include "mbed.h"


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); 
}