xxxxxx

Dependencies:   mbed

Fork of FINAL 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.025;  // 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     }
00093 }
00094 
00095 void init_TIMER()
00096 {
00097     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds)
00098 }
00099 
00100 void init_UART()
00101 {
00102     bt.baud(115200);  // baud rate設為115200
00103     bt.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
00104 }
00105 
00106 void timer1_ITR()
00107 {
00108     // 避免收到錯誤資料,若超出設定範圍則用上次的資料
00109     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) {
00110         data_received[0] = data_received_old[0];
00111         data_received[1] = data_received_old[1];
00112         data_received[2] = data_received_old[2];
00113     } else {
00114         data_received_old[0] = data_received[0];
00115         data_received_old[1] = data_received[1];
00116         data_received_old[2] = data_received[2];
00117     }
00118 
00119     if( data_received_old[2] == 1)
00120     {
00121         //open
00122         // servo_duty = xxx;
00123     }
00124     
00125     if( data_received_old[2] == 2)
00126     {
00127         //closed
00128         // servo_duty = xxx;
00129     }
00130     
00131     if(servo_duty >= 0.113f)servo_duty = 0.113;
00132     else if(servo_duty <= 0.025f)servo_duty = 0.025;
00133     servo_cmd.write(servo_duty);
00134     
00135    
00136     
00137     // motor1
00138     rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00139     speed_count_1 = 0;
00140 
00141     ///PI controller for motor1///
00142     
00143     err_1=(data_received_old[0]-rotation_speed_1)*p_gain;
00144     ierr_1=(err_1_old+err_1)*i_gain;
00145     PI_out_1=err_1+ierr_1;
00146     err_1_old=err_1;
00147     //////////////////////////////
00148     
00149     if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
00150     else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
00151     pwm1_duty = PI_out_1 + 0.5f;
00152     pwm1.write(pwm1_duty);
00153     TIM1->CCER |= 0x4;
00154 
00155     //motor2
00156     rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
00157     speed_count_2 = 0;
00158 
00159     ///PI controller for motor2///
00160     
00161      err_2=(data_received_old[1]-rotation_speed_2)*p_gain;
00162      ierr_2=(err_2_old+err_2)*i_gain;
00163      PI_out_2=err_2+ierr_2;
00164      err_2_old=err_2;
00165     //////////////////////////////
00166     
00167     if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
00168     else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
00169     pwm2_duty = -PI_out_2 + 0.5f;
00170     pwm2.write(pwm2_duty);
00171     TIM1->CCER |= 0x40;
00172     
00173     //pc.printf("SPEED= %f,/n",data_received_old[1]);
00174     
00175 }
00176 
00177 void RX_ITR()
00178 {
00179     while(bt.readable()) {
00180         static char uart_read;
00181         uart_read = bt.getc();
00182         if(uart_read == 127 && RX_flag1 == 1) {
00183             RX_flag2 = 1;
00184         } else {
00185             RX_flag1 = 0;
00186         }
00187 
00188         if(RX_flag2 == 1) {
00189             getData[readcount] = uart_read;
00190             readcount++;
00191             if(readcount >= 5) {
00192                 readcount = 0;
00193                 RX_flag2 = 0;
00194                 ///code for decoding///
00195                 data_received[0] = (getData[2] << 8) | getData[1]; 
00196                 data_received[1] = (getData[4] << 8) | getData[3];
00197                 data_received[2] = getData[5];
00198                 ///////////////////////
00199             }
00200         } 
00201         else if(uart_read == 254 && RX_flag1 == 0) {
00202             RX_flag1 = 1;
00203         }
00204     }
00205 }
00206     
00207 ///////////////////////////////////////////////////////////////////
00208 ///////////////////////////////////////////////////////////////////
00209 ///////////////////////////////////////////////////////////////////
00210 void init_PWM()
00211 {
00212     servo_cmd.period_ms(20);
00213     servo_cmd.write(servo_duty);
00214 
00215     pwm1.period_us(50);
00216     pwm1.write(0.5);
00217     TIM1->CCER |= 0x4;
00218 
00219     pwm2.period_us(50);
00220     pwm2.write(0.5);
00221     TIM1->CCER |= 0x40;
00222 }
00223 void init_CN()
00224 {
00225     HallA.rise(&CN_ITR);
00226     HallA.fall(&CN_ITR);
00227     HallB.rise(&CN_ITR);
00228     HallB.fall(&CN_ITR);
00229 
00230     HallA_2.rise(&CN_ITR);
00231     HallA_2.fall(&CN_ITR);
00232     HallB_2.rise(&CN_ITR);
00233     HallB_2.fall(&CN_ITR);
00234 }
00235 
00236 void CN_ITR()
00237 {
00238     
00239     // motor1
00240     HallA_1_state = HallA.read();
00241     HallB_1_state = HallB.read();
00242     //led1 != led1; 
00243 
00244     ///code for state determination///
00245     //////////////////////////////////
00246     //////////////////////////////////
00247     //determine the state
00248     if((HallA_1_state == 0)&&(HallB_1_state == 0))
00249     {
00250         state_1 = 1;
00251     }
00252     else if((HallA_1_state == 0)&&(HallB_1_state == 1))
00253     {
00254         state_1 = 2;
00255     }
00256     else if((HallA_1_state == 1)&&(HallB_1_state == 1))
00257     {
00258         state_1 = 3;
00259     }
00260     else if((HallA_1_state == 1)&&(HallB_1_state ==0))
00261     {
00262         state_1 = 4;
00263     }
00264     
00265     //forward or backward
00266     int direction_1 = 0;
00267     direction_1 = state_1 - state_1_old;
00268     if((direction_1 == -1) || (direction_1 == 3))
00269     {
00270         //forward
00271         speed_count_1 = speed_count_1 - 1;
00272     }
00273     else if((direction_1 == 1) || (direction_1 == -3))
00274     {
00275         //backward
00276         speed_count_1 = speed_count_1 + 1;
00277     }
00278     else
00279     {
00280         //prevent initializing error
00281         state_1_old = state_1;
00282     }
00283     
00284     //change old state
00285     state_1_old = state_1;
00286     //////////////////////////////////
00287     //////////////////////////////////
00288     //forward : speed_count_1 + 1
00289     //backward : speed_count_1 - 1
00290 
00291     // motor2
00292     HallA_2_state = HallA_2.read();
00293     HallB_2_state = HallB_2.read();
00294 
00295     ///code for state determination///
00296     //////////////////////////////////
00297     /////////////////////////////////
00298     //determine the state
00299     if((HallA_2_state == 0)&&(HallB_2_state == 0))
00300     {
00301         state_2 = 1;
00302     }
00303     else if((HallA_2_state == 0)&&(HallB_2_state == 1))
00304     {
00305         state_2 = 2;
00306     }
00307     else if((HallA_2_state == 1)&&(HallB_2_state == 1))
00308     {
00309         state_2 = 3;
00310     }
00311     else if((HallA_2_state == 1)&&(HallB_2_state ==0))
00312     {
00313         state_2 = 4;
00314     }
00315     
00316     //forward or backward
00317     int direction_2 = 0;
00318     direction_2 = state_2 - state_2_old;
00319     if((direction_2 == 1) || (direction_2 == -3))
00320     {
00321         //forward
00322         speed_count_2 = speed_count_2 - 1;
00323     }
00324     else if((direction_2 == -1) || (direction_2 == 3))
00325     {
00326         //backward
00327         speed_count_2 = speed_count_2 + 1;
00328     }
00329     else
00330     {
00331         //prevent initializing error
00332         state_2_old = state_2;
00333     }
00334     
00335     //change old state
00336     state_2_old = state_2;
00337     //////////////////////////////////
00338     //////////////////////////////////
00339     //forward : speed_count_2 + 1
00340     //backward : speed_count_2 - 1
00341 }