igloo hall
Dependencies: mbed
Fork of Robotics_LAB_motor by
main.cpp
- Committer:
- money24617111
- Date:
- 2017-03-21
- Revision:
- 1:b222972c5930
- Parent:
- 0:74ea99c4db88
File content as of revision 1:b222972c5930:
#include "mbed.h" Serial pc(USBTX,USBRX); //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) Ticker timer1; // servo motor PwmOut servo_cmd(A0); // DC motor PwmOut pwm1(D7); PwmOut pwm1n(D11); PwmOut pwm2(D8); PwmOut pwm2n(A3); // Motor1 sensor InterruptIn HallA(A1); InterruptIn HallB(A2); // Motor2 sensor InterruptIn HallA_2(D13); InterruptIn HallB_2(D12); // 函式宣告 void init_IO(); void init_TIMER(); void timer1_ITR(); void init_CN(); void CN_ITR(); void init_PWM(); // servo motor float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 int angle = 0; // Hall sensor int HallA_1_state = 0; int HallB_1_state = 0; int state_1 = 0; int state_1_old = 0; int HallA_2_state = 0; int HallB_2_state = 0; int state_2 = 0; int state_2_old = 0; // DC motor rotation speed control int speed_count_1 = 0; float rotation_speed_1 = 0.0; float rotation_speed_ref_1 = 150; float pwm1_duty = 0.5; float PI_out_1 = 0.0; float err_1 = 0.0; float ierr_1 = 0.0; int speed_count_2 = 0; float rotation_speed_2 = 0.0; float rotation_speed_ref_2 = 150; float pwm2_duty = 0.5; float PI_out_2 = 0.0; float err_2 = 0.0; float ierr_2 = 0.0; float Ki = 0.02; float Kp = 0.7; int main() { // FILE *fp = fopen("C:/Users/user/Desktop/out.txt", "w"); // Open "out.txt" on the local file system for writing init_TIMER(); init_PWM(); init_CN(); while(1) { } } void init_TIMER() { timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds) } void init_PWM() { servo_cmd.period_ms(20); servo_cmd.write(servo_duty); pwm1.period_us(50); pwm1.write(0.5f); TIM1->CCER |= 0x4; pwm2.period_us(50); pwm2.write(0.5f); TIM1->CCER |= 0x40; } void init_CN() { HallA.rise(&CN_ITR); HallA.fall(&CN_ITR); HallB.rise(&CN_ITR); HallB.fall(&CN_ITR); HallA_2.rise(&CN_ITR); HallA_2.fall(&CN_ITR); HallB_2.rise(&CN_ITR); HallB_2.fall(&CN_ITR); } void CN_ITR() { // motor1 HallA_1_state = HallA.read(); HallB_1_state = HallB.read(); // motor2 HallA_2_state = HallA_2.read(); HallB_2_state = HallB_2.read(); ///code for state determination/// // state determine (hall1) // if(HallA_1_state==0 && HallB_1_state ==0) { state_1 = 0; } else if(HallA_1_state==1 && HallB_1_state ==0) { state_1 = 1; } else if(HallA_1_state==0 && HallB_1_state ==1) { state_1 = 2; } else if(HallA_1_state==1 && HallB_1_state ==1) { state_1 = 3; } // end state determine of 1 // // foward and backward determine of 1// switch(state_1) { case 0 : if(state_1_old == 1) { speed_count_1 ++; // foward } else if(state_1_old == 2) { speed_count_1 --; // reverse } else{} break; case 1 : if(state_1_old == 3) { speed_count_1 ++; // foward } else if(state_1_old == 0) { speed_count_1 --; // reverse } else{} break; case 2 : if(state_1_old == 0) { speed_count_1 ++; // foward } else if(state_1_old == 3) { speed_count_1 --; // reverse } else{} break; case 3 : if(state_1_old == 2) { speed_count_1 ++; // foward } else if(state_1_old == 1) { speed_count_1 --; // reverse } else{} break; } // end foward and reverse determine of 1 // // update// state_1_old = state_1; // end update // // state determine (hall2) // if(HallA_2_state==0 && HallB_2_state ==0) { state_2 = 0; } else if(HallA_2_state==1 && HallB_2_state ==0) { state_2 = 1; } else if(HallA_2_state==0 && HallB_2_state ==1) { state_2 = 2; } else if(HallA_2_state==1 && HallB_2_state ==1) { state_2 = 3; } // end state determine of 2// // foward and reverse determine of 2// switch(state_2) { case 0 : if(state_2_old == 1) { speed_count_2 ++; // foward } else if(state_2_old == 2) { speed_count_2 --; // reverse } else{} break; case 1 : if(state_2_old == 3) { speed_count_2 ++; // foward } else if(state_2_old == 0) { speed_count_2 --; // reverse } else{} break; case 2 : if(state_2_old == 0) { speed_count_2 ++; // foward } else if(state_2_old == 3) { speed_count_2 --; // reverse } else{} break; case 3 : if(state_2_old == 2) { speed_count_2 ++; // foward } else if(state_2_old == 1) { speed_count_2 --; // reverse } else{} break; } // end foward and backward determine of 1 // // update// state_2_old = state_2; // end update // ////////////////////////////////// //forward : speed_count_1 + 1 //backward : speed_count_1 - 1 //forward : speed_count_2 + 1 //backward : speed_count_2 - 1 } void timer1_ITR() { // servo motor ///code for sevo motor/// ///////////////////////// if(servo_duty >= 0.113f)servo_duty = 0.113; else if(servo_duty <= 0.025f)servo_duty = 0.025; servo_cmd.write(servo_duty); // motor1 rotation_speed_1 = speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm pc.printf("%d\r\n",speed_count_1); speed_count_1 = 0; ///PI controller for motor1/// err_1 = Kp *(rotation_speed_ref_1 - rotation_speed_1); ierr_1 = ierr_1 + Ki*(rotation_speed_ref_1- rotation_speed_1)*0.01; PI_out_1 = (err_1 + ierr_1)*0.002; ////////////////////////////// if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; pwm1_duty = PI_out_1 + 0.5f; pwm1.write(pwm1_duty); TIM1->CCER |= 0x4; //motor2 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm speed_count_2 = 0; ///PI controller for motor2/// err_2 = float(Kp *(rotation_speed_ref_2 - rotation_speed_2)); ierr_2 = float(ierr_1 + Ki*(rotation_speed_ref_2- rotation_speed_2)*0.01); PI_out_2 = float((rotation_speed_2+err_2 + ierr_2)*0.002); ////////////////////////////// if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; pwm2_duty = PI_out_2 + 0.5f; pwm2.write(pwm2_duty); TIM1->CCER |= 0x40; }