Diff: main.cpp

Revision:

0:6b47dfc0d6aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 12 13:29:03 2018 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#define SERVO_PWM_PERIOD 20  //ms
+#define MOTOR_PWM_PERIOD 50  //us
+#define SERVO_INITIAL_DUTYCYCLE 0.05
+#define MOTOR_INITIAL_DUTYCYCLE 0.5
+
+
+#define PI 3.14159265
+
+
+
+DigitalOut led_1(PC_0);
+DigitalOut led_2(PC_1);
+Ticker servoTimer;
+Ticker motorTimer;
+PwmOut servo_cmd(PA_0);
+PwmOut motor_cmd_1(D7);
+PwmOut motor_cmd_1N(D11);
+
+
+
+void motorTimer_interrupt();
+void init_servo();
+void init_led();
+void servoTimer_interrupt();
+void init_timer();
+
+
+int main() {
+    init_led();
+    init_servo();
+    init_timer();
+
+    
+    while(1)
+    {
+        ;
+    }
+
+}
+
+void init_led()
+{
+    led_1 = 0;
+    led_2 = 0;    
+}
+
+
+
+void init_servo()
+{
+    servo_cmd.period_ms(SERVO_PWM_PERIOD);
+    servo_cmd.write(SERVO_INITIAL_DUTYCYCLE);
+    
+    
+    motor_cmd_1.period_us(MOTOR_PWM_PERIOD);
+    motor_cmd_1.write(MOTOR_INITIAL_DUTYCYCLE);
+    TIM1->CCER |= 0x4;
+
+}
+
+void init_timer(void)
+{
+    servoTimer.attach(&servoTimer_interrupt, 1.0); //1s
+    motorTimer.attach(&motorTimer_interrupt, 3.0); //1s
+}
+
+void servoTimer_interrupt()
+{
+    static double dutyCycle = SERVO_INITIAL_DUTYCYCLE;
+    static int n=0;
+    
+    //在這裡用7是為了讓一開始可以矯正
+    if((n!=0) && (n<7))
+    {
+        dutyCycle += 0.007333;  
+    }
+    else if ((7<=n) && (n<13))
+    {
+        dutyCycle -= 0.007333;  
+    }
+    servo_cmd.write(dutyCycle);
+    n++;
+    
+    if (n>=13)
+    {
+        n = 1;
+    }
+}
+
+
+void motorTimer_interrupt()
+{   
+    static int n = 0;
+    static float dutyCycle = 0.5;
+    
+    n++;
+    if (n >= 6)
+        n--;
+    dutyCycle = MOTOR_INITIAL_DUTYCYCLE + n*0.1;
+    motor_cmd_1.write(dutyCycle);
+    TIM1->CCER |= 0x4;
+    
+}