Diff: Servo_X.cpp

Revision:

4:4b884116fbe6

Parent:

3:07bcb3c9a8b8

Child:

6:c81ba0f175ac

--- a/Servo_X.cpp	Tue Mar 06 17:14:05 2018 +0000
+++ b/Servo_X.cpp	Thu Mar 08 02:44:07 2018 +0000
@@ -2,49 +2,61 @@
 #include "mbed.h"
 #include "Serial.h"         //serial
 
+
 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);
+    mypwmX.pulsewidth_us(MAXPULSE);
+    mypwmY.pulsewidth_us(MAXPULSE);
     mypwmZ.pulsewidth_us(NODRAW);
     
 }
+
 Servo::~Servo()
 {
 }
-int Servo::mm2pulse(int vmm)
+int Servo::mm2pulse(float vmm)
 {
   if (vmm < MAXPOS)
-    return vmm*(MAXPULSE-MINPULSE)/MAXPOS +MINPULSE;
-  return MAXPULSE;   
+    return int (vmm*(MAXPULSE-MINPULSE)/(MAXPOS)) +MINPULSE;
+  return MAXPULSE; 
 }
-void Servo::SetZ(int _z)
+void Servo::SetZ(uint8_t _z)
 {
     z=_z;
-    mypwmZ.pulsewidth_us(mm2pulse(z)); 
+    int PULSEZ=mm2pulse(CONVERTIDOR(z));
+    mypwmZ.pulsewidth_us(PULSEZ); 
 }
-void Servo::SetServo(int _x,int _y)
+void Servo::SetServo(uint8_t _x,uint8_t _y)
 {
     x=_x;
     y=_y;
-    mypwmX.pulsewidth_us(mm2pulse(x));
-    mypwmY.pulsewidth_us(mm2pulse(y));   
+    int PULSEX=mm2pulse(CONVERTIDOR(x));
+    int PULSEY=mm2pulse(CONVERTIDOR(y));
+    mypwmX.pulsewidth_us(PULSEX);
+    mypwmY.pulsewidth_us(PULSEY);   
 }
-int Servo::GetServoX()
+uint8_t Servo::GetServoX()
 {
     return x;
 }
-int Servo::GetServoY()
+uint8_t Servo::GetServoY()
 {
     return y;
 }
-int Servo::GetServoZ()
+uint8_t Servo::GetServoZ()
 {
     return z;
 }
+int Servo::CONVERTIDOR(int c)
+{
+    int u=0;
+    u=c/16;
+    return c-6*u;
+}