Diff: sm_servo.cpp

Revision:

3:1b7eb426247e

Parent:

2:e9d928fd327a

Child:

4:efa207509f63

--- a/sm_servo.cpp	Sat Sep 28 22:58:02 2019 +0000
+++ b/sm_servo.cpp	Sun Sep 29 00:43:58 2019 +0000
@@ -2,29 +2,28 @@
 
 PwmOut pwm_Servo(PE_9);
 E_STATE_SERVO e_stateServo;
-
+bool directionCheck = false;
 
 void init_sm_servo()
 {
-    pc.printf("init servo with pulse %d us",SERVO_PULSE_MIDDLE_US);
-    pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
-    pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
     e_stateServo = SERVO_INIT;
 }
 
 void update_sm_servo()
 {
- E_STATE_SERVO next_state;
+    E_STATE_SERVO next_state = e_stateServo;
 
     switch(e_stateServo) {
         case SERVO_INIT:
-            next_state = SERVO_COMMAND;
+            if(directionCheck) {
+                next_state = SERVO_COMMAND;
+            }
             break;
         case SERVO_COMMAND:
             break;
         default:
-        break;
-         }
+            break;
+    }
 
     e_stateServo = next_state;
     return;
@@ -32,5 +31,28 @@
 
 void output_sm_servo()
 {
-return;
+    switch(e_stateServo) {
+        case SERVO_INIT:
+            wait(1);
+            pc.printf("init servo with pulse %d us\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:
+            break;
+        default:
+            break;
+    }
+    return;
 }
\ No newline at end of file