YICHUN SAVE US

Dependencies:   mbed

Fork of FINALFINALNORMAL by Robotics ^___^

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

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