welcome referance

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.015;
00074 float i_gain=0.003;
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     // motor1
00249     HallA_1_state = HallA.read();
00250     HallB_1_state = HallB.read();
00251 
00252     ///code for state determination///
00253     if(state_1==1 && state_1_old==1){
00254         if(HallA_1_state==1 && HallB_1_state==0){
00255                 speed_count_1=speed_count_1 + 1;
00256             }
00257         else if(HallA_1_state==0 && HallB_1_state==1){
00258                 speed_count_1=speed_count_1 - 1;
00259             }
00260         }
00261     if(state_1==1 && state_1_old==0){
00262         if(HallA_1_state==0 && HallB_1_state==0){
00263                 speed_count_1=speed_count_1 + 1;
00264             }
00265         else if(HallA_1_state==1 && HallB_1_state==1){
00266                 speed_count_1=speed_count_1 - 1;
00267             }
00268         }
00269     if(state_1==0 && state_1_old==0){
00270         if(HallA_1_state==0 && HallB_1_state==1){
00271                 speed_count_1=speed_count_1 + 1;
00272             }
00273         else if(HallA_1_state==1 && HallB_1_state==0){
00274                 speed_count_1=speed_count_1 - 1;
00275             }
00276         }
00277     if(state_1==0 && state_1_old==1){
00278         if(HallA_1_state==1 && HallB_1_state==1){
00279                 speed_count_1=speed_count_1 + 1;
00280             }
00281         else if(HallA_1_state==0 && HallB_1_state==0){
00282                 speed_count_1=speed_count_1 - 1;
00283             }
00284         }
00285     state_1=HallA_1_state;
00286     state_1_old=HallB_1_state;
00287     //////////////////////////////////
00288 
00289     //forward : speed_count_1 + 1
00290     //backward : speed_count_1 - 1
00291 
00292 
00293     // motor2
00294     HallA_2_state = HallA_2.read();
00295     HallB_2_state = HallB_2.read();
00296 
00297     ///code for state determination///
00298     if(state_2==1 && state_2_old==1){
00299         if(HallA_2_state==1 && HallB_2_state==0){
00300                 speed_count_2=speed_count_2 + 1;
00301             }
00302         else if(HallA_2_state==0 && HallB_2_state==1){
00303                 speed_count_2=speed_count_2 - 1;
00304             }
00305         }
00306     if(state_2==1 && state_2_old==0){
00307         if(HallA_2_state==0 && HallB_2_state==0){
00308                 speed_count_2=speed_count_2 + 1;
00309             }
00310         else if(HallA_2_state==1 && HallB_2_state==1){
00311                 speed_count_2=speed_count_2 - 1;
00312             }
00313         }
00314     if(state_2==0 && state_2_old==0){
00315         if(HallA_2_state==0 && HallB_2_state==1){
00316                 speed_count_2=speed_count_2 + 1;
00317             }
00318         else if(HallA_2_state==1 && HallB_2_state==0){
00319                 speed_count_2=speed_count_2 - 1;
00320             }
00321         }
00322     if(state_2==0 && state_2_old==1){
00323         if(HallA_2_state==1 && HallB_2_state==1){
00324                 speed_count_2=speed_count_2 + 1;
00325             }
00326         else if(HallA_2_state==0 && HallB_2_state==0){
00327                 speed_count_2=speed_count_2 - 1;
00328             }
00329         }
00330     state_2=HallA_2_state;
00331     state_2_old=HallB_2_state;
00332 
00333 
00334     //////////////////////////////////
00335 
00336     //forward : speed_count_2 + 1
00337     //backward : speed_count_2 - 1
00338 }