Revision 0:5e356103dcc7, committed 2018-04-12

Comitter:

farmookong

Date:

Thu Apr 12 09:59:57 2018 +0000

Commit message:

LAB4SERVOOK

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 12 09:59:57 2018 +0000
@@ -0,0 +1,269 @@
+/* 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 Servo_Period 20
+//****************************************************************************** End of Define
+ 
+//****************************************************************************** I/O
+//PWM
+Serial pc(USBTX, USBRX); // tx, rx
+ 
+//Dc motor
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+PwmOut servo(A0);
+//Motor1 sensor
+InterruptIn HallA_1(A1);
+InterruptIn HallB_1(A2);
+//Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+ 
+//LED
+DigitalOut led1(A4);
+DigitalOut led2(A5);
+ 
+//Timer Setting
+Ticker timer;
+//****************************************************************************** End of I/O
+ 
+//****************************************************************************** Functions
+void init_timer(void);
+void init_CN(void);
+void init_PWM(void);
+void timer_interrupt(void);
+void CN_interrupt(void);
+//****************************************************************************** End of Functions
+ 
+//****************************************************************************** Variables
+// Servo
+float servo_duty = 0.03; // 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;
+//servo
+int i = 0;
+//****************************************************************************** End of Variables
+ 
+//****************************************************************************** Main
+int main()
+{
+    init_timer();
+    init_PWM();
+    init_CN();
+    while(1)
+    {
+        pc.printf("**************************\n");       
+        pc.printf("speed1: %f, error: %f\n",speed_1,v_err_1);
+        pc.printf("i_error: %f ctrl output: %f \n",v_ierr_1,ctrl_output_1);
+        pc.printf("speed2: %f, error: %f\n",speed_2,v_err_2);
+        pc.printf("i_error: %f ctrl output: %f \n",v_ierr_2,ctrl_output_2);
+ 
+    }
+}
+//****************************************************************************** End of Main
+ 
+//****************************************************************************** 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 //
+    v_err_1 = v_ref_1 - speed_1;
+    v_ierr_1 += (v_err_1*Ts);
+    ctrl_output_1 = 0.01f*v_err_1+ 0.1f*v_ierr_1; 
+    ///////////////////////////
+    
+    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;
+    
+    // Motor2
+    speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
+    count_2 = 0;
+    // Code for PI controller //
+    v_err_2 = v_ref_2 - speed_2;
+    v_ierr_2 += (v_err_2*Ts);
+    ctrl_output_2 = 0.001f*v_err_2+ 0.05f*v_ierr_2;
+    ///////////////////////////      
+    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;
+    
+    if(v_ierr_1 > 5)
+        v_ierr_1 = 0;
+    if(v_ierr_2 > 8)
+        v_ierr_2 = 0;
+        
+    //Servo
+    if(i==100) 
+    {
+        if(servo_duty < 0.07f)
+        {
+            servo_duty = servo_duty+0.04f/6;
+        }
+        else
+        {
+            servo_duty = 0.07f;            
+        }
+        servo.write(servo_duty);
+        i=0;
+    }
+    else
+    {
+        i++;
+    }
+}
+//****************************************************************************** End of timer_interrupt
+ 
+//****************************************************************************** CN_interrupt
+void CN_interrupt()
+{
+    // Motor1
+    // Read the current status of hall sensor
+    HallA_state_1 = HallA_1.read();
+    HallB_state_1 = HallB_1.read();
+     
+   ///code for state determination///
+    if(HallA_state_1 == 0 && HallB_state_1 == 0)
+        motor_state_1 = 1;
+    else if(HallA_state_1 == 0 && HallB_state_1 == 1)
+        motor_state_1 = 2;
+    else if(HallA_state_1 == 1 && HallB_state_1 == 1)
+        motor_state_1 = 3;
+    else if(HallA_state_1 == 1 && HallB_state_1 == 0)
+        motor_state_1 = 4;
+    
+    if(motor_state_old_1 != 0)
+    {
+        if(motor_state_old_1 < motor_state_1)
+            count_1 += 1;
+        else if(motor_state_old_1 > motor_state_1)
+            count_1 -= 1;
+        if(motor_state_old_1 == 4 && motor_state_1 == 1)
+            count_1 += 2;
+        if(motor_state_old_1 == 1 && motor_state_1 == 4)
+            count_1 -= 2;
+    }
+    motor_state_old_1 = motor_state_1;
+    //////////////////////////////////
+    
+    //Forward
+    //v1Count +1
+    //Inverse
+    //v1Count -1
+        
+    // 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(HallA_state_2 == 0 && HallB_state_2 == 0)
+        motor_state_2 = 1;
+    else if(HallA_state_2 == 0 && HallB_state_2 == 1)
+        motor_state_2 = 2;
+    else if(HallA_state_2 == 1 && HallB_state_2 == 1)
+        motor_state_2 = 3;
+    else if(HallA_state_2 == 1 && HallB_state_2 == 0)
+        motor_state_2 = 4;
+    
+    if(motor_state_old_2 != 0)
+    {
+        if(motor_state_old_2 < motor_state_2)
+            count_2 += 1;
+        else if(motor_state_old_2 > motor_state_2)
+            count_2 -= 1;
+        if(motor_state_old_2 == 4 && motor_state_2 == 1)
+            count_2 += 2;
+        if(motor_state_old_2 == 1 && motor_state_2 == 4)
+            count_2 -= 2;
+    }
+    motor_state_old_2 = motor_state_2;
+    
+    //////////////////////////////////
+    
+    //Forward
+    //v2Count +1
+    //Inverse
+    //v2Count -1
+}
+//****************************************************************************** End of CN_interrupt
+ 
+//****************************************************************************** init_timer
+void init_timer()
+{
+     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);
+    TIM1->CCER |= 0x4;
+    
+    pwm2.period_us(50);
+    pwm2.write(0.5);
+    TIM1->CCER |= 0x40;
+    
+    servo.period_ms(Servo_Period);
+    servo.write(servo_duty);
+}
+//****************************************************************************** End of init_PWM
+ 
+//****************************************************************************** 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);
+    
+    HallA_state_2 = HallA_2.read();
+    HallB_state_2 = HallB_2.read();
+}
+//****************************************************************************** End of init_CN
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Apr 12 09:59:57 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/994bdf8177cb
\ No newline at end of file