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@45:501b7909139a, 2017-03-30 (annotated)

Committer:
hazheng
Date:
Thu Mar 30 03:50:23 2017 +0000
Revision:
45:501b7909139a
Parent:
20:4cdbd1ae4ead
Child:
46:a5eb9bd3bb55
Turn servo class into pure C code. More classes need to be changed on tomorrow...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bobymicjohn 11:676ea42afd56 1 #include "Servo.h"
Bobymicjohn 11:676ea42afd56 2
Bobymicjohn 11:676ea42afd56 3 #include "Core.h"
Bobymicjohn 11:676ea42afd56 4 #include "PinAssignment.h"
Bobymicjohn 11:676ea42afd56 5 #include "SWUSBServer.h"
Bobymicjohn 11:676ea42afd56 6
Bobymicjohn 11:676ea42afd56 7 #include "mbed.h"
Bobymicjohn 11:676ea42afd56 8
hazheng 45:501b7909139a 9 #ifdef __cplusplus
hazheng 45:501b7909139a 10 extern "C" {
hazheng 45:501b7909139a 11 #endif
Bobymicjohn 11:676ea42afd56 12
hazheng 45:501b7909139a 13 static float servo_pulseWidth = 0.0015;
hazheng 45:501b7909139a 14 static PwmOut servo_pwm(PIN_SC_SERVO);
hazheng 45:501b7909139a 15
hazheng 45:501b7909139a 16 void servo_init()
hazheng 45:501b7909139a 17 {
hazheng 45:501b7909139a 18 servo_pwm.period(0.020);
hazheng 45:501b7909139a 19 servo_pwm.pulsewidth(servo_pulseWidth);
hazheng 45:501b7909139a 20 }
hazheng 45:501b7909139a 21
hazheng 45:501b7909139a 22 void servo_set_angle(float angle)
hazheng 45:501b7909139a 23 {
hazheng 45:501b7909139a 24 if(angle > SERVO_RT * SERVO_MAX_ANGLE)
hazheng 45:501b7909139a 25 angle = SERVO_RT * SERVO_MAX_ANGLE;
hazheng 45:501b7909139a 26 else if(angle < SERVO_LF * SERVO_MAX_ANGLE)
hazheng 45:501b7909139a 27 angle = SERVO_LF * SERVO_MAX_ANGLE;
hazheng 45:501b7909139a 28
hazheng 45:501b7909139a 29 servo_pulseWidth = (0.0015 + (0.00001667 * angle));
hazheng 45:501b7909139a 30 //char buf[10];
hazheng 45:501b7909139a 31 //sprintf(buf, "%f", m_pulseWidth);
hazheng 45:501b7909139a 32 //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 45:501b7909139a 33 servo_pwm.pulsewidth(servo_pulseWidth);
hazheng 45:501b7909139a 34 }
hazheng 45:501b7909139a 35
hazheng 45:501b7909139a 36 #ifdef __cplusplus
hazheng 45:501b7909139a 37 }
hazheng 45:501b7909139a 38 #endif
hazheng 45:501b7909139a 39 /*
Bobymicjohn 11:676ea42afd56 40 Servo::Servo(SW::Core& core) :
Bobymicjohn 11:676ea42afd56 41 m_core(core),
hazheng 13:7dcb1642ef99 42 m_pwm(PwmOut(PIN_SC_SERVO))
Bobymicjohn 11:676ea42afd56 43
Bobymicjohn 11:676ea42afd56 44 {
Bobymicjohn 11:676ea42afd56 45 m_pulseWidth = 0.0015;
Bobymicjohn 11:676ea42afd56 46 m_pwm.period(0.020);
Bobymicjohn 11:676ea42afd56 47 m_pwm.pulsewidth(m_pulseWidth);
Bobymicjohn 11:676ea42afd56 48 }
Bobymicjohn 11:676ea42afd56 49
Bobymicjohn 11:676ea42afd56 50
Bobymicjohn 11:676ea42afd56 51 void Servo::Update(float deltaTime)
Bobymicjohn 11:676ea42afd56 52 {
Bobymicjohn 11:676ea42afd56 53
Bobymicjohn 11:676ea42afd56 54 }
Bobymicjohn 11:676ea42afd56 55
Bobymicjohn 11:676ea42afd56 56 void Servo::setAngle(float angle)
Bobymicjohn 11:676ea42afd56 57 {
Bobymicjohn 11:676ea42afd56 58 if(angle > SERVO_RT * SERVO_MAX_ANGLE)
Bobymicjohn 11:676ea42afd56 59 angle = SERVO_RT * SERVO_MAX_ANGLE;
Bobymicjohn 11:676ea42afd56 60 else if(angle < SERVO_LF * SERVO_MAX_ANGLE)
Bobymicjohn 11:676ea42afd56 61 angle = SERVO_LF * SERVO_MAX_ANGLE;
Bobymicjohn 11:676ea42afd56 62
Bobymicjohn 11:676ea42afd56 63 m_pulseWidth = (0.0015 + (0.00001667 * angle));
hazheng 16:66c7a09e71ee 64 //char buf[10];
hazheng 16:66c7a09e71ee 65 //sprintf(buf, "%f", m_pulseWidth);
hazheng 16:66c7a09e71ee 66 //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
Bobymicjohn 11:676ea42afd56 67 m_pwm.pulsewidth(m_pulseWidth);
Bobymicjohn 11:676ea42afd56 68 }
hazheng 45:501b7909139a 69 */