Revision 3:0856a9fa97ec, committed 2018-03-19

Comitter:

cpul5338

Date:

Mon Mar 19 08:20:03 2018 +0000

Parent:

2:9caf46a5fe5a

Commit message:

Robotics LAB DCMotor

Changed in this revision

--- a/main.cpp	Wed May 25 07:02:22 2016 +0000
+++ b/main.cpp	Mon Mar 19 08:20:03 2018 +0000
@@ -1,23 +1,21 @@
-/*LAB_DCMotor*/
+/* LAB DCMotor */
 #include "mbed.h"
 
+//****************************************************************************** Define
 //The number will be compiled as type "double" in default
 //Add a "f" after the number can make it compiled as type "float"
 #define Ts 0.01f    //period of timer1 (s)
-#define Kp 0.0025f
-#define Ki 0.008f
+
+//****************************************************************************** End of Define
 
+//****************************************************************************** I/O
+//PWM
+//Dc motor
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
 PwmOut pwm2n(A3);
 
-Serial bluetooth(D10,D2);
-Serial pc(D1, D0);
-
-DigitalOut led1(A4);
-DigitalOut led2(A5);
-
 //Motor1 sensor
 InterruptIn HallA_1(A1);
 InterruptIn HallB_1(A2);
@@ -25,175 +23,103 @@
 InterruptIn HallA_2(D13);
 InterruptIn HallB_2(D12);
 
-Ticker timer1;
-void timer1_interrupt(void);
-void CN_interrupt(void);
+//LED
+DigitalOut led1(A4);
+DigitalOut led2(A5);
 
-void init_TIMER(void);
+//Timer Setting
+Ticker timer;
+//****************************************************************************** End of I/O
+
+//****************************************************************************** Functions
+void init_timer(void);
+void init_CN(void);
 void init_PWM(void);
-void init_CN(void);
-void init_err(void);
-
-int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
-int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
+void timer_interrupt(void);
+void CN_interrupt(void);
+//****************************************************************************** End of Functions
 
-int v1Count = 0;
-int v2Count = 0;
+//****************************************************************************** Variables
+// Servo
+float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
+// motor 1
+int8_t HallA_state_1 = 0;
+int8_t HallB_state_1 = 0;
+int8_t motor_state_1 = 0;
+int8_t motor_state_old_1 = 0;
+int count_1 = 0;
+float speed_1 = 0.0f;
+float v_ref_1 = 80.0f;
+float v_err_1 = 0.0f;
+float v_ierr_1 = 0.0f;
+float ctrl_output_1 = 0.0f;
+float pwm1_duty = 0.0f;
+//motor 2
+int8_t HallA_state_2 = 0;
+int8_t HallB_state_2 = 0;
+int8_t motor_state_2 = 0;
+int8_t motor_state_old_2 = 0;
+int count_2 = 0;
+float speed_2 = 0.0f;
+float v_ref_2 = 150.0f;
+float v_err_2 = 0.0f;
+float v_ierr_2 = 0.0f;
+float ctrl_output_2 = 0.0f;
+float pwm2_duty = 0.0f;
+//****************************************************************************** End of Variables
 
-float v1 = 0.0, v1_ref = 0.0;
-float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
-float v2 = 0.0, v2_ref = 0.0;
-float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
-
-int main() {
-    
-    init_TIMER();
+//****************************************************************************** Main
+int main()
+{
+    init_timer();
     init_PWM();
     init_CN();
-    
-    bluetooth.baud(115200); //設定鮑率
-    pc.baud(57600);
-    
-    
-    while(1) 
+    while(1)
     {
-        if(pc.readable())
-        {
-            bluetooth.putc(pc.getc());
-        }
-        if(bluetooth.readable())
-        {
-            pc.putc(bluetooth.getc());
-        }
     }
 }
+//****************************************************************************** End of Main
 
-void timer1_interrupt(void)
-{
-    //Motor 1
-    v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
-    v1Count = 0;
+//****************************************************************************** timer_interrupt
+void timer_interrupt()
+{   
+    // Motor1
+    speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
+    count_1 = 0;
+    // Code for PI controller //
     
-    ///code for PI control///    
-    v1_err = v1_ref - v1;
-    v1_ierr = Ts*v1_err + v1_ierr;
-    PIout_1 = Kp*v1_err + Ki*v1_ierr; 
+    ///////////////////////////
     
-    /////////////////////////
-    
-    if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
-    else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
-    pwm1.write(PIout_1 + 0.5f);
+    if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
+    else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
+    pwm1_duty = ctrl_output_1 + 0.5f;
+    pwm1.write(pwm1_duty);
     TIM1->CCER |= 0x4;
     
-    
-    //Motor 2
-    v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
-    v2Count = 0;
+    // Motor2
+    speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
+    count_2 = 0;
+    // Code for PI controller //
     
-    ///code for PI control///
-    v2_err = v2_ref - v2;
-    v2_ierr = Ts*v2_err + v2_ierr;
-    PIout_2 = Kp*v2_err + Ki*v2_ierr; 
-
-    /////////////////////////
-    
-    if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
-    else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
-    pwm2.write(PIout_2 + 0.5f);
-    TIM1->CCER |= 0x40;
+    ///////////////////////////    
     
-    switch(bluetooth.getc())
-    {
-    case '1':
-        v1_ref = 30;
-        v2_ref = 30;
-        init_err();
-        break;
-    case '2':
-        v1_ref = 40;
-        v2_ref = 40;
-        init_err();
-        break;
-    case '3':
-        v1_ref = 50;
-        v2_ref = 50;
-        init_err();
-        break;
-    case '4':
-        v1_ref = 60;
-        v2_ref = 60;
-        init_err();
-        break;
-    case '5':
-        v1_ref = 70;
-        v2_ref = 70;
-        init_err();
-        break;
-    case '6':
-        v1_ref = 80;
-        v2_ref = 80;
-        init_err();
-        break;
-    case '7':
-        v1_ref = 100;
-        v2_ref = 100;
-        init_err();
-        break;
-    case '8':
-        v1_ref = 0;
-        v2_ref = 0;
-        break;            
-    }
-    
-    
+    if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
+    else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
+    pwm2_duty = ctrl_output_2 + 0.5f;
+    pwm2.write(pwm2_duty);
+    TIM1->CCER |= 0x40;
 }
+//****************************************************************************** End of timer_interrupt
 
-void CN_interrupt(void)
+//****************************************************************************** CN_interrupt
+void CN_interrupt()
 {
-    //Motor 1
-    stateA_1 = HallA_1.read();
-    stateB_1 = HallB_1.read();
-    
-    ///code for state determination///
-    if(stateA_1==0&&stateB_1==0){
-    state_1 = 1;}
-    else if(stateA_1==0&&stateB_1==1){
-    state_1 = 2;}
-    else if(stateA_1==1&&stateB_1==1){
-    state_1 = 3;}
-    else if(stateA_1==1&&stateB_1==0){
-    state_1 = 4;}
-       
-    if(state_1 == 1)
-    {
-        if(state_1-state_1_old == -3)
-        v1Count=v1Count+1;
-        else if(state_1-state_1_old == -1)
-        v1Count=v1Count-1;     
-    }
-    else if(state_1 == 2)
-    {
-        if(state_1-state_1_old == 1)
-        v1Count=v1Count+1;
-        else if(state_1-state_1_old == -1)
-        v1Count=v1Count-1;     
-    }
-    else if(state_1 == 3)
-    {
-        if(state_1-state_1_old == 1)
-        v1Count=v1Count+1;
-        else if(state_1-state_1_old == -1)
-        v1Count=v1Count-1;     
-    }
-    else if(state_1 == 4)
-    {
-        if(state_1-state_1_old == 1)
-        v1Count=v1Count+1;
-        else if(state_1-state_1_old == 3)
-        v1Count=v1Count-1;     
-    } 
-        state_1_old = state_1;
+    // Motor1
+    // Read the current status of hall sensor
+    HallA_state_1 = HallA_1.read();
+    HallB_state_1 = HallB_1.read();
+     
+   ///code for state determination///
     
     
     //////////////////////////////////
@@ -202,51 +128,14 @@
     //v1Count +1
     //Inverse
     //v1Count -1
-    
-    
-    //Motor 2
-    stateA_2 = HallA_2.read();
-    stateB_2 = HallB_2.read();
-    
+        
+    // Motor2
+    // Read the current status of hall sensor
+    HallA_state_2 = HallA_2.read();
+    HallB_state_2 = HallB_2.read();
+     
     ///code for state determination///
-    if(stateA_2==0&&stateB_2==0){
-    state_2 = 1;}
-    else if(stateA_2==0&&stateB_2==1){
-    state_2 = 2;}
-    else if(stateA_2==1&&stateB_2==1){
-    state_2 = 3;}
-    else if(stateA_2==1&&stateB_2==0){
-    state_2 = 4;}
     
-    if(state_2 == 1)
-    {
-        if(state_2-state_2_old == -3)
-        v2Count=v2Count+1;
-        else if(state_2-state_2_old == -1)
-        v2Count=v2Count-1;     
-    }
-    else if(state_2 == 2)
-    {
-        if(state_2-state_2_old == 1)
-        v2Count=v2Count+1;
-        else if(state_2-state_2_old == -1)
-        v2Count=v2Count-1;     
-    }
-    else if(state_2 == 3)
-    {
-        if(state_2-state_2_old == 1)
-        v2Count=v2Count+1;
-        else if(state_2-state_2_old == -1)
-        v2Count=v2Count-1;     
-    }
-    else if(state_2 == 4)
-    {
-        if(state_2-state_2_old == 1)
-        v2Count=v2Count+1;
-        else if(state_2-state_2_old == 3)
-        v2Count=v2Count-1;     
-    }
-        state_2_old = state_2;
     
     //////////////////////////////////
     
@@ -255,13 +144,17 @@
     //Inverse
     //v2Count -1
 }
+//****************************************************************************** End of CN_interrupt
 
-void init_TIMER(void)
+//****************************************************************************** init_timer
+void init_timer()
 {
-    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
-}         
-   
-void init_PWM(void)
+     timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
+}
+//****************************************************************************** End of init_timer
+
+//****************************************************************************** init_PWM
+void init_PWM()
 {
     pwm1.period_us(50);
     pwm1.write(0.5);
@@ -271,26 +164,27 @@
     pwm2.write(0.5);
     TIM1->CCER |= 0x40;
 }
+//****************************************************************************** End of init_PWM
 
-void init_CN(void)
+//****************************************************************************** init_CN
+void init_CN()
 {
+    // Motor1
     HallA_1.rise(&CN_interrupt);
     HallA_1.fall(&CN_interrupt);
     HallB_1.rise(&CN_interrupt);
     HallB_1.fall(&CN_interrupt);
     
+    HallA_state_1 = HallA_1.read();
+    HallB_state_1 = HallB_1.read();
+    
+    // Motor2
     HallA_2.rise(&CN_interrupt);
     HallA_2.fall(&CN_interrupt);
     HallB_2.rise(&CN_interrupt);
     HallB_2.fall(&CN_interrupt);
     
-    stateA_1 = HallA_1.read();
-    stateB_1 = HallB_1.read();
-    stateA_2 = HallA_2.read();
-    stateB_2 = HallB_2.read();
+    HallA_state_2 = HallA_2.read();
+    HallB_state_2 = HallB_2.read();
 }
-void init_err(void)
-{
-    v1_ierr = 0.0;    
-    v2_ierr = 0.0;
-}
\ No newline at end of file
+//****************************************************************************** End of init_CN
\ No newline at end of file