final

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