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-07
Revision:
11:676ea42afd56
Child:
13:7dcb1642ef99

File content as of revision 11:676ea42afd56:

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

/*
void Servo::turnRight(float angle)
{
    if(angle > 21)
        angle = 21;
    
    m_pulseWidth = (0.0015 + (0.000017 * angle));
    m_pwm.pulsewidth(m_pulseWidth); 
}

void Servo::turnLeft(float angle)
{
    if(angle > 21)
        angle = 21;
        
    m_pulseWidth = (0.0015 - (0.00017 * angle));
    m_pwm.pulsewidth(m_pulseWidth); 
}
*/