Diff: Servo_X.cpp

Revision:

2:cc30b42ac66f

Child:

3:07bcb3c9a8b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo_X.cpp	Mon Feb 26 19:38:14 2018 +0000
@@ -0,0 +1,48 @@
+#include "Servo_X.h"
+#include "mbed.h"
+
+PwmOut mypwmX(PA_8);
+PwmOut mypwmY(PB_10);
+PwmOut mypwmZ(PB_4);
+Servo::Servo()
+{
+    mypwmX.period_ms(20);
+    mypwmY.period_ms(20);
+    mypwmZ.period_ms(20);
+    mypwmX.pulsewidth_us(MINPULSE);
+    mypwmY.pulsewidth_us(MINPULSE);
+    mypwmZ.pulsewidth_us(NODRAW);
+    
+}
+Servo::~Servo(){
+}
+int Servo::mm2pulse(int vmm)
+{
+  if (vmm < MAXPOS)
+    return vmm*(MAXPULSE-MINPULSE)/MAXPOS +MINPULSE;
+  return MAXPULSE;   
+}
+void Servo::SetZ(int _z)
+{
+    z=_z;
+    mypwmZ.pulsewidth_us(mm2pulse(z)); 
+}
+void Servo::SetServo(int _x,int _y)
+{
+    x=_x;
+    y=_y;
+    mypwmX.pulsewidth_us(mm2pulse(x));
+    mypwmY.pulsewidth_us(mm2pulse(y));   
+}
+int Servo::GetServoX()
+{
+    return x;
+}
+int Servo::GetServoY()
+{
+    return y;
+}
+int Servo::GetServoZ()
+{
+    return z;
+}