keyboard.cpp

00001 /* LAB DCMotor */
00002 #include "mbed.h"
00003 #include <ros.h>
00004 #include <geometry_msgs/Vector3.h>
00005 #include <geometry_msgs/Twist>
00006  
00007 //****************************************************************************** Define
00008 //The number will be compiled as type "double" in default
00009 //Add a "f" after the number can make it compiled as type "float"
00010 #define Ts 0.01f    //period of timer1 (s)
00011  
00012 //****************************************************************************** End of Define
00013  
00014 //****************************************************************************** I/O
00015 //PWM
00016 //Dc motor
00017 PwmOut pwm1(D7);
00018 PwmOut pwm1n(D11);
00019 PwmOut pwm2(D8);
00020 PwmOut pwm2n(A3);
00021  
00022 //Motor1 sensor
00023 InterruptIn HallA_1(A1);
00024 InterruptIn HallB_1(A2);
00025 //Motor2 sensor
00026 InterruptIn HallA_2(D13);
00027 InterruptIn HallB_2(D12);
00028  
00029 //LED
00030 DigitalOut led1(A4);
00031 DigitalOut led2(A5);
00032  
00033 //Timer Setting
00034 Ticker timer;
00035 //****************************************************************************** End of I/O
00036  
00037 //****************************************************************************** Functions
00038 void init_timer(void);
00039 void init_CN(void);
00040 void init_PWM(void);
00041 void timer_interrupt(void);
00042 void CN_interrupt(void);
00043 //****************************************************************************** End of Functions
00044  
00045 //****************************************************************************** Variables
00046 // Servo
00047 float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
00048 
00049 // motor 1
00050 int8_t HallA_state_1 = 0;
00051 int8_t HallB_state_1 = 0;
00052 int8_t motor_state_1 = 0;
00053 int8_t motor_state_old_1 = 0;
00054 int count_1 = 0;
00055 float speed_1 = 0.0f;
00056 float v_ref_1 = 80.0f;
00057 float v_err_1 = 0.0f;     //   v_err_old_1 = v_err_1 ;  v_err_1 = v_ref_1 - speed_1 ;
00058 float v_ierr_1 = 0.0f;   //integral error : v_ierr_1 = v_err_old_1 + v_err_1;
00059 float ctrl_output_1 = 0.0f;
00060 float pwm1_duty = 0.0f;
00061 
00062 float Kp_1 = 50;  //need to be tested
00063 float Ki_1 = 100;  //need to be tested
00064 float ctrl_output_old_1 = 0.0f;
00065 
00066 
00067 //motor 2
00068 int8_t HallA_state_2 = 0;
00069 int8_t HallB_state_2 = 0;
00070 int8_t motor_state_2 = 0;
00071 int8_t motor_state_old_2 = 0;
00072 int count_2 = 0;
00073 float speed_2 = 0.0f;
00074 float v_ref_2 = 150.0f;
00075 float v_err_2 = 0.0f;
00076 float v_ierr_2 = 0.0f;
00077 float ctrl_output_2 = 0.0f;
00078 float pwm2_duty = 0.0f;
00079 
00080 
00081 float Kp_2 = 50;
00082 float Ki_2 = 100;
00083 float ctrl_output_old_2 = 0.0f;
00084 
00085 
00086 //****************************************************************************** End of Variables
00087  
00088  
00089 //****************************************************************************** Main
00090 int main()
00091 {
00092     init_PWM();
00093     init_timer();
00094     init_CN();
00095     while(1)
00096     {
00097     }
00098 }
00099 //****************************************************************************** End of Main
00100  
00101 //****************************************************************************** timer_interrupt
00102 void timer_interrupt()
00103 {   
00104     // Motor1
00105     speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm of output wheel (period=0.01 sec, each period has 12 segments, reduction ratio 29)
00106     count_1 = 0;
00107     // Code for PI controller //
00108     
00109       
00110     v_err_1 = v_ref_1 - speed_1 ;
00111     
00112     ctrl_output_1 = (ctrl_output_old_1) + (Kp_1+Ki_1*Ts*0.5f)*(v_err_1) + (-Kp_1+Ki_1)*0.5f*(v_ierr_1);
00113     
00114     v_ierr_1 =  v_err_1 ;
00115     ctrl_output_old_1 = ctrl_output_1;
00116 
00117     
00118     ///////////////////////////
00119     
00120     if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
00121     else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
00122     pwm1_duty = ctrl_output_1 + 0.5f;
00123     pwm1.write(pwm1_duty);
00124     TIM1->CCER |= 0x4;
00125     
00126     
00127     
00128     // Motor2
00129     speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
00130     count_2 = 0;
00131     // Code for PI controller //
00132       
00133     v_err_2 = v_ref_2 - speed_2 ;
00134     
00135     ctrl_output_2 = (ctrl_output_old_2) + (Kp_2+Ki_2*Ts*0.5f)*(v_err_2) + (-Kp_2+Ki_2)*0.5f*(v_ierr_2);
00136     
00137     v_ierr_2 =  v_err_2 ;
00138     ctrl_output_old_2 = ctrl_output_2;
00139     
00140     ///////////////////////////    
00141     
00142     if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
00143     else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
00144     pwm2_duty = ctrl_output_2 + 0.5f;
00145     pwm2.write(pwm2_duty);
00146     TIM1->CCER |= 0x40;
00147     
00148 }
00149 //****************************************************************************** End of timer_interrupt
00150  
00151 //****************************************************************************** CN_interrupt
00152 void CN_interrupt()
00153 {
00154     // Motor1
00155     // Read the current status of hall sensor
00156     HallA_state_1 = HallA_1.read();
00157     HallB_state_1 = HallB_1.read();
00158      
00159    ///code for state determination///
00160     if(HallA_state_1==0)
00161     {
00162         if(HallB_state_1==0)
00163         {
00164         motor_state_1 =1;
00165         }
00166         else
00167         {
00168          motor_state_1 =2;   
00169             }
00170         }      
00171     else
00172     {
00173         if(HallB_state_1==0)
00174         {
00175             motor_state_1 = 3;
00176             }
00177             else
00178             {
00179                 motor_state_1 = 4;
00180                 }
00181         }
00182          
00183     
00184     //////////////////////////////////
00185     switch(motor_state_1)
00186     {
00187         case 1:
00188             if(motor_state_old_1 == 4) 
00189             count_1--;           
00190             else if(motor_state_old_1 == 2) 
00191             count_1++;  
00192             break; 
00193         case 2:
00194             if(motor_state_old_1 == 1) 
00195             count_1--;                 
00196             else if(motor_state_old_1 == 3) 
00197             count_1++;  
00198             break; 
00199         case 3:
00200             if(motor_state_old_1== 2)  
00201             count_1--;                 
00202             else if(motor_state_old_1 == 4) 
00203             count_1++;  
00204             break; 
00205         case 4:
00206             if(motor_state_old_1 == 3) 
00207             count_1--;  
00208             else if(motor_state_old_1 == 1) 
00209             count_1++;  
00210             break; 
00211     }
00212     motor_state_old_1 = motor_state_1;
00213     //Forward
00214     //v1Count +1
00215     //Inverse
00216     //v1Count -1
00217         
00218     // Motor2
00219     // Read the current status of hall sensor
00220     HallA_state_2 = HallA_2.read();
00221     HallB_state_2 = HallB_2.read();
00222      
00223     ///code for state determination///
00224    if(HallA_state_2==0)
00225     {
00226         if(HallB_state_2==0)
00227         {
00228         motor_state_2 =1;
00229         }
00230         else
00231         {
00232          motor_state_2 =2;   
00233             }
00234         }        
00235     else
00236     {
00237         if(HallB_state_2==0)
00238         {
00239             motor_state_2 = 3;
00240             }
00241             else
00242             {
00243                 motor_state_2 = 4;
00244                 }
00245         }
00246       
00247     //////////////////////////////////
00248         switch(motor_state_2)
00249     {
00250         case 1:
00251             if(motor_state_old_2 == 4) 
00252             count_2--;           
00253             else if(motor_state_old_2 == 2) 
00254             count_2++;  
00255             break; 
00256         case 2:
00257             if(motor_state_old_2 == 1) 
00258             count_1--;                 
00259             else if(motor_state_old_2 == 3) 
00260             count_2++;  
00261             break; 
00262         case 3:
00263             if(motor_state_old_2== 2)  
00264             count_2--;                 
00265             else if(motor_state_old_2 == 4) 
00266             count_2++;  
00267             break; 
00268         case 4:
00269             if(motor_state_old_2 == 3) 
00270             count_2--;  
00271             else if(motor_state_old_2 == 1) 
00272             count_2++;  
00273             break; 
00274     }
00275     motor_state_old_2 = motor_state_2;
00276     
00277     //Forward
00278     //v2Count +1
00279     //Inverse
00280     //v2Count -1
00281 }
00282 //****************************************************************************** End of CN_interrupt
00283  
00284 //****************************************************************************** init_timer
00285 void init_timer()
00286 {
00287      timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
00288 }
00289 //****************************************************************************** End of init_timer
00290  
00291 //****************************************************************************** init_PWM
00292 void init_PWM()
00293 {
00294     pwm1.period_us(50);
00295     pwm1.write(0.5);
00296     TIM1->CCER |= 0x4;
00297     
00298     pwm2.period_us(50);
00299     pwm2.write(0.5);
00300     TIM1->CCER |= 0x40;
00301 }
00302 //****************************************************************************** End of init_PWM
00303  
00304 //****************************************************************************** init_CN
00305 void init_CN()
00306 {
00307     // Motor1
00308     HallA_1.rise(&CN_interrupt);
00309     HallA_1.fall(&CN_interrupt);
00310     HallB_1.rise(&CN_interrupt);
00311     HallB_1.fall(&CN_interrupt);
00312     
00313 
00314     
00315     // Motor2
00316     HallA_2.rise(&CN_interrupt);
00317     HallA_2.fall(&CN_interrupt);
00318     HallB_2.rise(&CN_interrupt);
00319     HallB_2.fall(&CN_interrupt);
00320     
00321 
00322 }
00323 //****************************************************************************** End of init_CN