Diff: main.cpp

Revision:

0:aa6e86a5ffae

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 12 13:34:31 2018 +0000
@@ -0,0 +1,292 @@
+#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 Kp 0.1f
+#define Ki 0.03f
+#define K  125.0f
+#define DESIRE_REV_1  50.0f  //rpm
+
+#define PI 3.14159265f
+
+enum STATE{ONE, TWO, THREE, FOUR, DEFAULT};
+
+//DigitalOut led_1(PC_0);
+//DigitalOut led_2(PC_1);
+Ticker servoTimer;
+Ticker motorTimer;
+PwmOut servo_cmd(PA_0);
+PwmOut motor_cmd_1(D8);
+PwmOut motor_cmd_1N(A3);
+
+InterruptIn hallA_1(D13);
+InterruptIn hallB_1(D12);
+
+int v1Count = 0;
+
+
+void init_servo();
+//void init_led();
+void servoTimer_interrupt();
+void motorTimer_interrupt();
+void init_timer();
+void init_hall();
+void CN_interrupt();
+
+int main() {
+//    init_led();
+    init_servo();
+    init_timer();
+    init_hall();
+    
+    while(1)
+    {
+        ;
+    }
+
+}
+/*
+void init_led()
+{
+    led_1 = 0;
+    led_2 = 0;    
+}
+*/
+void init_hall()
+{
+    hallA_1.rise(&CN_interrupt);
+    hallB_1.rise(&CN_interrupt);
+
+    
+    hallA_1.fall(&CN_interrupt);
+    hallB_1.fall(&CN_interrupt);
+
+}
+
+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_us(&motorTimer_interrupt, 10000.0); //10ms
+}
+
+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 float rev_1 = 0.0;
+
+    static float output_vol_1 = 0.0;
+
+    static float error_rev_1 = 0.0;
+
+    static float intergral_1 = 0.0;
+
+    static float duty_cycle_1 = 0.0;
+
+    
+    ////motor1輸出
+    rev_1 = (float)v1Count /12.0f *6000.0f /29.0f;  //rpm
+    error_rev_1 = (DESIRE_REV_1 -rev_1)*2.0f*PI/60.0f;  // (rad/s)
+    intergral_1 += error_rev_1 *0.01f;
+    output_vol_1 = error_rev_1*Kp + intergral_1*Ki;
+
+    duty_cycle_1 = (5.0f +output_vol_1) /10.0f;
+    
+    //如果dutyCycle超過極值的話，讓led亮
+    if ( (0 <= duty_cycle_1) && (duty_cycle_1 <= 1) )
+    {
+        motor_cmd_1.write(duty_cycle_1);
+        TIM1->CCER |= 0x4;
+    }
+    else
+    {
+        //led_1 = 1;
+    }
+    
+    
+    
+    
+    //在這裡把Count都歸零，因為Count是累加的，如果不歸零轉速會一直變大
+    v1Count = 0;
+
+    
+}
+
+void CN_interrupt()
+{
+    static bool hall1A_state_1;
+    static bool hall1B_state_1;
+
+    static STATE old_state_1 = DEFAULT;
+    static STATE new_state_1 = DEFAULT;
+
+
+    //0是正轉，1是反轉
+    static bool motor_dirc_1;
+
+    //0是沒變，1是有變
+    static bool state_change_1 = 1;
+
+
+    ////////////////////motor1///////////////////////
+    hall1A_state_1 = hallA_1.read();
+    hall1B_state_1 = hallB_1.read();
+    old_state_1 = new_state_1;
+    
+    if (hall1A_state_1 == 0)
+    {
+        if (hall1B_state_1 == 0)
+        {
+            new_state_1 = ONE;
+        }
+        else if(hall1B_state_1 == 1)
+        {
+            new_state_1 = TWO;
+        }
+    }
+    else if (hall1A_state_1 == 1)
+    {
+        if (hall1B_state_1 == 0)
+        {
+            new_state_1 = THREE;
+        }
+        else if(hall1B_state_1 == 1)
+        {
+            new_state_1 = FOUR;
+        }
+    }
+    
+    ///////判斷正反轉//////
+    switch (old_state_1)
+    {
+        case ONE:
+            if(new_state_1 == TWO)
+            {
+                motor_dirc_1 = 0;     
+            }
+            else if(new_state_1 == FOUR)
+            {        
+                motor_dirc_1 = 1;               
+            }
+            else if(new_state_1 == ONE)
+            {
+                state_change_1 = 0;
+            }
+            else
+            {
+                //led_1 = 1;
+            }
+            break;
+        case TWO:
+            if(new_state_1 == THREE)
+            {
+                motor_dirc_1 = 0;     
+            }
+            else if(new_state_1 == ONE)
+            {        
+                motor_dirc_1 = 1;               
+            }
+            else if(new_state_1 == TWO)
+            {
+                state_change_1 = 0;
+            }
+            else
+            {
+                //led_1 = 1;
+            }
+            break;
+        case THREE:
+            if(new_state_1 == FOUR)
+            {
+                motor_dirc_1 = 0;     
+            }
+            else if(new_state_1 == TWO)
+            {        
+                motor_dirc_1 = 1;               
+            }
+            else if(new_state_1 == THREE)
+            {
+                state_change_1 = 0;
+            }
+            else
+            {
+                //led_1 = 1;
+            }
+            break;
+        case FOUR:
+            if(new_state_1 == ONE)
+            {
+                motor_dirc_1 = 0;     
+            }
+            else if(new_state_1 == THREE)
+            {        
+                motor_dirc_1 = 1;               
+            }
+            else if(new_state_1 == FOUR)
+            {
+                state_change_1 = 0;
+            }
+            else
+            {
+                //led_1 = 1;
+            }
+            break;
+        case DEFAULT:
+        
+            return ;
+             
+    }
+    
+    ///////上下計數///////
+    if (state_change_1)
+    {
+        if (motor_dirc_1 == 0)
+        {
+            v1Count += 1;  
+        }
+        else if (motor_dirc_1 == 1)
+        {
+             v1Count -= 1;   
+        }
+    }
+    
+    
+
+    
+    //這裡默認每次進來state都會改變
+    state_change_1 = 1;
+}