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:
46:a5eb9bd3bb55
Parent:
45:501b7909139a
Child:
87:15fcf7891bf9

File content as of revision 46:a5eb9bd3bb55:

#include "Servo.h"

#include "Core.h"
#include "PinAssignment.h"
#include "SWUSBServer.h"
#include "GlobalVariable.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));
    servo_pwm.pulsewidth(servo_pulseWidth); 
    //servo_pwm = (servo_pulseWidth / 0.020);
    
    //char buf[20];
    //sprintf(buf, "Serv %f", (float)servo_pwm);
    //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
}

#ifdef __cplusplus
}
#endif