Diff: sm_servo.cpp

Revision:

4:efa207509f63

Parent:

3:1b7eb426247e

Child:

6:ab9f3695633f

--- a/sm_servo.cpp	Sun Sep 29 00:43:58 2019 +0000
+++ b/sm_servo.cpp	Sun Sep 29 14:58:03 2019 +0000
@@ -3,6 +3,7 @@
 PwmOut pwm_Servo(PE_9);
 E_STATE_SERVO e_stateServo;
 bool directionCheck = false;
+int pulsewidth = SERVO_PULSE_MAX_US;
 
 void init_sm_servo()
 {
@@ -34,22 +35,29 @@
     switch(e_stateServo) {
         case SERVO_INIT:
             wait(1);
-            pc.printf("init servo with pulse %d us\n",SERVO_PULSE_MIDDLE_US);
+            pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
             pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
             pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
-            wait(1);
-            pc.printf("send pulse 1600 to servo\n");
-            pwm_Servo.pulsewidth_us(1600);
-            wait(1);
-            pc.printf("send pulse 1300 to servo\n");
-            pwm_Servo.pulsewidth_us(1300);
-            wait(1);
-            pc.printf("send pulse 1500 to servo\n");
-            pwm_Servo.pulsewidth_us(1500);
-            wait(1);
             directionCheck = true;
             break;
         case SERVO_COMMAND:
+        /*
+            if(pulsewidth < SERVO_PULSE_MAX_US && directionCheck) {
+                pulsewidth += 1;
+                if (pulsewidth >= SERVO_PULSE_MAX_US) {
+                    directionCheck = false;
+                }
+            }  else if (pulsewidth > SERVO_PULSE_MIN_US && !directionCheck) {
+                pulsewidth -= 1;
+                if (pulsewidth <= SERVO_PULSE_MIN_US) {
+                    directionCheck = true;
+                }
+            }
+            pc.printf("send pulse %d to servo\r\n",pulsewidth);
+            */
+            
+            pwm_Servo.pulsewidth_us(pulsewidth);
+
             break;
         default:
             break;