1

Dependencies:   mbed

Fork of Robotics_LAB_motor by NTHUPME_Robotics_2017

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 
00003 //The number will be compiled as type "double" in default
00004 //Add a "f" after the number can make it compiled as type "float"
00005 #define Ts 0.01f    //period of timer1 (s)
00006 
00007 Ticker timer1;
00008 // servo motor
00009 PwmOut servo_cmd(A0);
00010 // DC motor
00011 PwmOut pwm1(D7);
00012 PwmOut pwm1n(D11);
00013 PwmOut pwm2(D8);
00014 PwmOut pwm2n(A3);
00015 
00016 // Motor1 sensor
00017 InterruptIn HallA(A1);
00018 InterruptIn HallB(A2);
00019 // Motor2 sensor
00020 InterruptIn HallA_2(D13);
00021 InterruptIn HallB_2(D12);
00022 
00023 // 函式宣告
00024 void init_IO();
00025 void init_TIMER();
00026 void timer1_ITR();
00027 void init_CN();
00028 void CN_ITR();
00029 void init_PWM();
00030 
00031 // servo motor
00032 float servo_duty = 0.069;  // 0.069 +(0.088/180)*angle, -90<angle<90
00033 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
00034 int angle = 0;
00035 
00036 // Hall sensor
00037 int HallA_1_state = 0;
00038 int HallB_1_state = 0;
00039 int state_1 = 0;
00040 int state_1_old = 0;
00041 int HallA_2_state = 0;
00042 int HallB_2_state = 0;
00043 int state_2 = 0;
00044 int state_2_old = 0;
00045 
00046 // DC motor rotation speed control
00047 int speed_count_1 = 0;
00048 float rotation_speed_1 = 0.0;
00049 float rotation_speed_ref_1 = 0;
00050 float pwm1_duty = 0.5;
00051 float PI_out_1 = 0.0;
00052 float err_1 = 0.0;
00053 float ierr_1 = 0.0;
00054 int speed_count_2 = 0;
00055 float rotation_speed_2 = 0.0;
00056 float rotation_speed_ref_2 = 0;
00057 float pwm2_duty = 0.5;
00058 float PI_out_2 = 0.0;
00059 float err_2 = 0.0;
00060 float ierr_2 = 0.0;
00061 
00062 float Kp = 0.006;
00063 float Ki = 0.02;
00064 
00065 int main()
00066 {
00067     init_TIMER();
00068     init_PWM();
00069     init_CN();
00070 
00071     while(1) {
00072     }
00073 }
00074 
00075 void init_TIMER()
00076 {
00077     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
00078 }
00079 
00080 void init_PWM()
00081 {
00082     servo_cmd.period_ms(20);
00083     servo_cmd.write(servo_duty);
00084 
00085     pwm1.period_us(50);
00086     pwm1.write(0.5);
00087     TIM1->CCER |= 0x4;
00088 
00089     pwm2.period_us(50);
00090     pwm2.write(0.5);
00091     TIM1->CCER |= 0x40;
00092 }
00093 void init_CN()
00094 {
00095     HallA.rise(&CN_ITR);
00096     HallA.fall(&CN_ITR);
00097     HallB.rise(&CN_ITR);
00098     HallB.fall(&CN_ITR);
00099 
00100     HallA_2.rise(&CN_ITR);
00101     HallA_2.fall(&CN_ITR);
00102     HallB_2.rise(&CN_ITR);
00103     HallB_2.fall(&CN_ITR);
00104 }
00105 
00106 void CN_ITR()
00107 {
00108     // motor1
00109     HallA_1_state = HallA.read();
00110     HallB_1_state = HallB.read();
00111 
00112     ///code for state determination///
00113     state_1 = 10*HallA_1_state + HallB_1_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
00114    
00115    if(state_1_old != state_1)
00116    { 
00117         if((state_1_old/10) == (state_1_old%10))
00118         {
00119             if((state_1%10) != (state_1_old%10))
00120             {
00121                 speed_count_1++;
00122             }
00123             else if((state_1/10) != (state_1_old/10))
00124             {
00125                 speed_count_1--;
00126             }
00127         }
00128         else if((state_1_old/10) != (state_1_old%10))
00129         {
00130             if((state_1%10) != (state_1_old%10))
00131             {
00132                 speed_count_1--;
00133             }
00134             else if((state_1/10) != (state_1_old/10))
00135             {
00136                 speed_count_1++;
00137             }    
00138         }
00139     
00140         state_1_old = state_1;
00141     }
00142     
00143     //////////////////////////////////
00144 
00145     //forward : speed_count_1 + 1
00146     //backward : speed_count_1 - 1
00147 
00148 
00149     // motor2
00150     HallA_2_state = HallA_2.read();
00151     HallB_2_state = HallB_2.read();
00152 
00153     ///code for state determination///
00154     state_2 = 10*HallA_2_state + HallB_2_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
00155     
00156     if(state_2_old != state_2)
00157     {
00158         if((state_2_old/10) == (state_2_old%10))
00159         {
00160             if((state_2%10) != (state_2_old%10))
00161             {
00162                 speed_count_2++;
00163             }
00164             else if((state_2/10) != (state_2_old/10))
00165             {
00166                 speed_count_2--;
00167             }
00168         }
00169         else if((state_2_old/10) != (state_2_old%10))
00170         {
00171             if((state_2%10) != (state_2_old%10))
00172             {
00173                 speed_count_2--;
00174             }
00175             else if((state_2/10) != (state_2_old/10))
00176             {
00177                 speed_count_2++;
00178             }    
00179         }
00180     
00181         state_2_old = state_2;
00182     }
00183     //////////////////////////////////
00184 
00185     //forward : speed_count_2 + 1
00186     //backward : speed_count_2 - 1
00187 }
00188 
00189 void timer1_ITR()
00190 {
00191     // servo motor
00192     ///code for sevo motor///
00193     
00194     servo_duty += 11.0f/1500.0f;
00195     
00196     /////////////////////////
00197     
00198     if(servo_duty >= 0.113f)servo_duty = 0.113;
00199     else if(servo_duty <= 0.025f)servo_duty = 0.025;
00200     servo_cmd.write(servo_duty);
00201 
00202     // motor1
00203     rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00204     speed_count_1 = 0;
00205 
00206     ///PI controller for motor1///
00207     rotation_speed_ref_1 = 150;
00208     err_1 = rotation_speed_ref_1 - rotation_speed_1;
00209     ierr_1 = ierr_1 + err_1 * Ts;
00210     PI_out_1 = Kp * err_1 + Ki * ierr_1;
00211     
00212     //////////////////////////////
00213     
00214     if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
00215     else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
00216     pwm1_duty = PI_out_1 + 0.5f;
00217     pwm1.write(pwm1_duty);
00218     TIM1->CCER |= 0x4;
00219 
00220     //motor2
00221     rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00222     speed_count_2 = 0;
00223 
00224     ///PI controller for motor2///
00225     rotation_speed_ref_2 = 150;
00226     err_2 = rotation_speed_ref_2 - rotation_speed_2;
00227     ierr_2 = ierr_2 + err_2 * Ts;
00228     PI_out_2 = Kp * err_2 + Ki * ierr_2;
00229     
00230     
00231     //////////////////////////////
00232     
00233     if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
00234     else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
00235     pwm2_duty = PI_out_2 + 0.5f;
00236     pwm2.write(pwm2_duty);
00237     TIM1->CCER |= 0x40;
00238 }