Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
farmookong
Date:
Thu Apr 12 09:59:57 2018 +0000
Commit message:
LAB4SERVOOK

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 5e356103dcc7 main.cpp
--- /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
diff -r 000000000000 -r 5e356103dcc7 mbed.bld
--- /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