lab6

Dependencies:   mbed

Fork of Robotics_LAB_UART 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 #define Ts 0.01f    //period of timer1 (s)
00004 #define Kp 0.005f
00005 #define Ki 0.02f
00006 
00007 Ticker timer1;
00008 Ticker timer2;
00009 Serial bt(D10, D2);  // TXpin, RXpin
00010 
00011 // servo motor
00012 PwmOut servo_cmd(A0);
00013 // DC motor
00014 PwmOut pwm1(D7);
00015 PwmOut pwm1n(D11);
00016 PwmOut pwm2(D8);
00017 PwmOut pwm2n(A3);
00018 
00019 // Motor1 sensor
00020 InterruptIn HallA(A1);
00021 InterruptIn HallB(A2);
00022 // Motor2 sensor
00023 InterruptIn HallA_2(D13);
00024 InterruptIn HallB_2(D12);
00025 
00026 // servo motor
00027 float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
00028 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
00029 int angle = 0;
00030 int counter;
00031 
00032 //RX
00033 int readcount = 0;
00034 int RX_flag1 = 0;
00035 int RX_flag2 = 0;
00036 char getData[6] = {0,0,0,0,0,0};
00037 short data_received[3] = {0,0,0};
00038 short data_received_old[3] = {0,0,0};
00039 
00040 //函式宣告
00041 void init_TIMER();
00042 void timer1_ITR();
00043 void timer2_ITR();
00044 void init_UART();
00045 void RX_ITR();
00046 void init_IO();
00047 void init_CN();
00048 void CN_ITR();
00049 void init_PWM();
00050 
00051 
00052 // Hall sensor
00053 int HallA_1_state = 0;
00054 int HallB_1_state = 0;
00055 int state_1 = 0;
00056 int state_1_old = 0;
00057 int HallA_2_state = 0;
00058 int HallB_2_state = 0;
00059 int state_2 = 0;
00060 int state_2_old = 0;
00061 
00062 // DC motor rotation speed control
00063 int speed_count_1 = 0;
00064 float rotation_speed_1 = 0.0;
00065 float rotation_speed_ref_1 = 0;
00066 float pwm1_duty = 0.5;
00067 float PI_out_1 = 0.0;
00068 float err_1 = 0.0;
00069 float ierr_1 = 0.0;
00070 int speed_count_2 = 0;
00071 float rotation_speed_2 = 0.0;
00072 float rotation_speed_ref_2 = 0;
00073 float pwm2_duty = 0.5;
00074 float PI_out_2 = 0.0;
00075 float err_2 = 0.0;
00076 float ierr_2 = 0.0;
00077 
00078 int main()
00079 {
00080     init_PWM();
00081     init_CN();
00082     init_TIMER();
00083     init_UART();
00084     while(1) {
00085     }
00086 }
00087 
00088 void init_TIMER()
00089 {
00090     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds)
00091     timer2.attach_us(&timer2_ITR, 10000.0);
00092 }
00093 
00094 void init_UART()
00095 {
00096     bt.baud(115200);  // baud rate設為115200
00097     bt.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
00098 }
00099 
00100 void timer1_ITR()
00101 {
00102     // 避免收到錯誤資料,若超出設定範圍則用上次的資料
00103     if(data_received[0]>300 || data_received[0]<-300 || data_received[1]>300 || data_received[1]<-300 || data_received[2]>90 || data_received[0]<-90) {
00104         data_received[0] = data_received_old[0];
00105         data_received[1] = data_received_old[1];
00106         data_received[2] = data_received_old[2];
00107     } else {
00108         data_received_old[0] = data_received[0];
00109         data_received_old[1] = data_received[1];
00110         data_received_old[2] = data_received[2];       
00111     }
00112     rotation_speed_ref_1 = data_received[0];
00113     rotation_speed_ref_2 = (-1)*data_received[1];
00114     
00115 }
00116 
00117 void RX_ITR()
00118 {
00119     while(bt.readable()) {
00120         static char uart_read;
00121         uart_read = bt.getc();
00122         if(uart_read == 127 && RX_flag1 == 1) {
00123             RX_flag2 = 1;
00124         } else {
00125             RX_flag1 = 0;
00126         }
00127 
00128         if(RX_flag2 == 1) {
00129             getData[readcount] = uart_read;
00130             readcount++;
00131             if(readcount >= 5) {
00132                 readcount = 0;
00133                 RX_flag2 = 0;
00134                 ///code for decoding///
00135                 data_received[0] = (getData[2] << 8) | getData[1];
00136                 data_received[1] = (getData[4] << 8) | getData[3];
00137                 data_received[2] = getData[5];
00138                 ///////////////////////
00139             }
00140         } else if(uart_read == 254 && RX_flag1 == 0) {
00141             RX_flag1 = 1;
00142         }
00143     }
00144 }
00145 
00146 void init_PWM()
00147 {
00148     servo_cmd.period_ms(20);
00149     servo_cmd.write(servo_duty);
00150 
00151     pwm1.period_us(50);
00152     pwm1.write(0.5);
00153     TIM1->CCER |= 0x4;
00154 
00155     pwm2.period_us(50);
00156     pwm2.write(0.5);
00157     TIM1->CCER |= 0x40;
00158 }
00159 void init_CN()
00160 {
00161     HallA.rise(&CN_ITR);
00162     HallA.fall(&CN_ITR);
00163     HallB.rise(&CN_ITR);
00164     HallB.fall(&CN_ITR);
00165 
00166     HallA_2.rise(&CN_ITR);
00167     HallA_2.fall(&CN_ITR);
00168     HallB_2.rise(&CN_ITR);
00169     HallB_2.fall(&CN_ITR);
00170 }
00171 
00172 void CN_ITR()
00173 {
00174     // motor1
00175     HallA_1_state = HallA.read();
00176     HallB_1_state = HallB.read();
00177 
00178     ///code for state determination///
00179 
00180    state_1 = 10*HallA_1_state + HallB_1_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
00181    
00182    if(state_1_old != state_1)
00183    { 
00184         if((state_1_old/10) == (state_1_old%10))
00185         {
00186             if((state_1%10) != (state_1_old%10))
00187             {
00188                 speed_count_1++;
00189             }
00190             else if((state_1/10) != (state_1_old/10))
00191             {
00192                 speed_count_1--;
00193             }
00194         }
00195         else if((state_1_old/10) != (state_1_old%10))
00196         {
00197             if((state_1%10) != (state_1_old%10))
00198             {
00199                 speed_count_1--;
00200             }
00201             else if((state_1/10) != (state_1_old/10))
00202             {
00203                 speed_count_1++;
00204             }    
00205         }
00206     
00207         state_1_old = state_1;
00208     }
00209    
00210 
00211     //////////////////////////////////
00212 
00213     //forward : speed_count_1 + 1
00214     //backward : speed_count_1 - 1
00215 
00216 
00217     // motor2
00218     HallA_2_state = HallA_2.read();
00219     HallB_2_state = HallB_2.read();
00220 
00221     ///code for state determination///
00222 
00223    state_2 = 10*HallA_2_state + HallB_2_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
00224     
00225     if(state_2_old != state_2)
00226     {
00227         if((state_2_old/10) == (state_2_old%10))
00228         {
00229             if((state_2%10) != (state_2_old%10))
00230             {
00231                 speed_count_2++;
00232             }
00233             else if((state_2/10) != (state_2_old/10))
00234             {
00235                 speed_count_2--;
00236             }
00237         }
00238         else if((state_2_old/10) != (state_2_old%10))
00239         {
00240             if((state_2%10) != (state_2_old%10))
00241             {
00242                 speed_count_2--;
00243             }
00244             else if((state_2/10) != (state_2_old/10))
00245             {
00246                 speed_count_2++;
00247             }    
00248         }
00249     
00250         state_2_old = state_2;
00251     }
00252    
00253 
00254  
00255     //////////////////////////////////
00256 
00257     //forward : speed_count_2 + 1
00258     //backward : speed_count_2 - 1
00259 }
00260 
00261 void timer2_ITR()
00262 {
00263     // servo motor
00264     ///code for sevo motor///
00265     
00266        counter = counter + 1;
00267     
00268     if(counter == 100)    
00269     {
00270         servo_duty = 0.069;       
00271     }
00272     if(counter == 200)    
00273     {
00274         servo_duty = 0.0763;        
00275     }
00276     if(counter == 300)    
00277     {
00278         servo_duty = 0.0837;        
00279     }
00280     if(counter == 400)    
00281     {
00282         servo_duty = 0.091;        
00283     }
00284     if(counter == 500)    
00285     {
00286         servo_duty = 0.0983;         
00287     }
00288     if(counter == 600)    
00289     {
00290         servo_duty = 0.106;     
00291     }      
00292  
00293     if(counter == 700)
00294     {
00295         servo_duty = 0.113;
00296     }
00297      if(counter > 700)
00298     {
00299       counter=0;
00300     }
00301     
00302     servo_cmd.write(servo_duty);
00303     
00304     
00305     /////////////////////////
00306     
00307     if(servo_duty >= 0.113f)servo_duty = 0.113;
00308     else if(servo_duty <= 0.025f)servo_duty = 0.025;
00309     servo_cmd.write(servo_duty);
00310 
00311     // motor1
00312     rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f ;   //unit: rpm
00313     speed_count_1 = 0;
00314 
00315     ///PI controller for motor1///
00316    
00317     err_1 = rotation_speed_ref_1 - rotation_speed_1;
00318     ierr_1 = ierr_1 + err_1*Ts;
00319     PI_out_1 = Kp*err_1 + Ki*ierr_1;
00320     
00321     //////////////////////////////
00322     
00323     if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
00324     else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
00325     pwm1_duty = PI_out_1 + 0.5f;
00326     pwm1.write(PI_out_1 + 0.5f);
00327     TIM1->CCER |= 0x4;
00328 
00329     //motor2
00330     rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00331     speed_count_2 = 0;
00332 
00333     ///PI controller for motor2///
00334     
00335     err_2 = rotation_speed_ref_2 - rotation_speed_2;
00336     ierr_2 = ierr_2 + err_2*Ts;
00337     PI_out_2 = Kp*err_2 + Ki*ierr_2;
00338     
00339     //////////////////////////////
00340     
00341     if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
00342     else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
00343     pwm2_duty = PI_out_2 + 0.5f;
00344     pwm2.write(PI_out_2 + 0.5f);
00345     TIM1->CCER |= 0x40;
00346 }