![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
哇
Dependencies: mbed
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
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