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

Diff: Hardwares/Servo.cpp

Revision:
45:501b7909139a
Parent:
20:4cdbd1ae4ead
Child:
46:a5eb9bd3bb55
--- a/Hardwares/Servo.cpp	Wed Mar 29 21:19:27 2017 +0000
+++ b/Hardwares/Servo.cpp	Thu Mar 30 03:50:23 2017 +0000
@@ -6,7 +6,37 @@
 
 #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))
@@ -36,3 +66,4 @@
     //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
     m_pwm.pulsewidth(m_pulseWidth); 
 }
+*/
\ No newline at end of file