main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <geometry_msgs/Vector3.h>
00004 #include <geometry_msgs/Twist.h>
00005 
00006 #define MOTOR_PWM_PERIOD 50  //us
00007 #define MOTOR_INITIAL_DUTYCYCLE 0.5f
00008 #define Kp 0.01f
00009 #define Ki 0.08f
00010 
00011 enum STATE{ONE, TWO, THREE, FOUR, DEFAULT};
00012 
00013 class HaseHardware : public MbedHardware
00014 {
00015 public:
00016     HaseHardware(): MbedHardware(D10, D2, 115200){};            
00017 };
00018 ros::NodeHandle_<HaseHardware> nh;
00019 
00020 Ticker motorTimer;
00021 PwmOut motor_cmd_1(D7);
00022 PwmOut motor_cmd_1N(D11);
00023 PwmOut motor_cmd_2(D8);
00024 PwmOut motor_cmd_2N(A3);
00025 InterruptIn hallA_1(A1);
00026 InterruptIn hallB_1(A2);
00027 InterruptIn hallA_2(D13);
00028 InterruptIn hallB_2(D12);
00029 
00030 
00031 geometry_msgs::Twist vel_msg;
00032 ros::Publisher p("feedback_wheel_angularVel", &vel_msg);
00033 
00034 
00035 int v1Count = 0;
00036 int v2Count = 0;
00037 float DESIRE_REV_1 = 0.0f;
00038 float DESIRE_REV_2 = 0.0f;
00039 float rev_1 = 0.0;
00040 float rev_2 = 0.0;
00041 
00042 void init_motor();
00043 void motorTimer_interrupt();
00044 void init_timer();
00045 void init_hall();
00046 void CN_interrupt();
00047 
00048 
00049 void messageCb(const geometry_msgs::Vector3& msg)
00050 {
00051     //receive velocity command from PC to motor
00052     DESIRE_REV_1 = msg.x;
00053     DESIRE_REV_2 = msg.y; 
00054 }
00055 ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel", messageCb);
00056 
00057 int main() {
00058     
00059     init_motor();
00060     init_timer();
00061     init_hall();
00062     
00063     nh.initNode();
00064     nh.subscribe(s);
00065     nh.advertise(p);
00066     
00067     while(1)
00068     {
00069         vel_msg.linear.x = DESIRE_REV_1;
00070         vel_msg.linear.y = rev_1;
00071         vel_msg.linear.z = 0.0f;
00072          
00073         vel_msg.angular.x = DESIRE_REV_2;
00074         vel_msg.angular.y = rev_2;
00075         vel_msg.angular.z = 0.0f;
00076         
00077         p.publish(&vel_msg);
00078         nh.spinOnce();
00079         wait_ms(50);
00080     }
00081 }
00082 
00083 void init_hall()
00084 {
00085     hallA_1.rise(&CN_interrupt);
00086     hallB_1.rise(&CN_interrupt);
00087     hallA_2.rise(&CN_interrupt);
00088     hallB_2.rise(&CN_interrupt);
00089     
00090     hallA_1.fall(&CN_interrupt);
00091     hallB_1.fall(&CN_interrupt);
00092     hallA_2.fall(&CN_interrupt);
00093     hallB_2.fall(&CN_interrupt);
00094 }
00095 
00096 void init_motor()
00097 {
00098     motor_cmd_1.period_us(MOTOR_PWM_PERIOD);
00099     motor_cmd_1.write(MOTOR_INITIAL_DUTYCYCLE);
00100     TIM1->CCER |= 0x4;
00101     motor_cmd_2.period_us(MOTOR_PWM_PERIOD);
00102     motor_cmd_2.write(MOTOR_INITIAL_DUTYCYCLE);
00103     TIM1->CCER |= 0x40;
00104 }
00105 
00106 void init_timer(void)
00107 {
00108 
00109     motorTimer.attach_us(&motorTimer_interrupt, 10000.0); //10ms
00110 }
00111 
00112 
00113 
00114 void motorTimer_interrupt()
00115 {
00116     
00117     static float output_vol_1 = 0.0;
00118     static float output_vol_2 = 0.0;
00119     static float error_rev_1 = 0.0;
00120     static float error_rev_2 = 0.0;
00121     static float intergral_1 = 0.0;
00122     static float intergral_2 = 0.0;
00123     static float duty_cycle_1 = 0.5;
00124     static float duty_cycle_2 = 0.5;
00125     
00126     ////motor1輸出
00127     
00128     rev_1 = (float)v1Count /12.0f *6000.0f /29.0f;  //rpm
00129     //在這裡把Count都歸零，因為Count是累加的，如果不歸零轉速會一直變大
00130     v1Count = 0;
00131     
00132     error_rev_1 = (DESIRE_REV_1 -rev_1);  // (rad/s)
00133     intergral_1 += error_rev_1 *0.01f;
00134     output_vol_1 = error_rev_1*Kp + intergral_1*Ki;
00135     
00136 
00137     if (-5.0f > output_vol_1)
00138     {
00139         output_vol_1 = -5.0f;
00140     }
00141     else if (output_vol_1 > 5.0f)
00142     {
00143         output_vol_1 = 5.0f;
00144     }
00145     duty_cycle_1 = (5.0f +output_vol_1) /10.0f;
00146     
00147     motor_cmd_1.write(duty_cycle_1);
00148     TIM1->CCER |= 0x4;
00149 
00150     
00151     
00152     ////motor2輸出
00153     
00154     rev_2 = (float)v2Count /12.0f *6000.0f /29.0f;  //rpm
00155     //在這裡把Count都歸零，因為Count是累加的，如果不歸零轉速會一直變大
00156     v2Count = 0;
00157     
00158     error_rev_2 = (DESIRE_REV_2 -rev_2);  // (rad/s)
00159     intergral_2 += error_rev_2 *0.01f;
00160     output_vol_2 = error_rev_2*Kp + intergral_2*Ki;
00161     
00162 
00163     if (-5.0f > output_vol_2)
00164     {
00165         output_vol_2 = -5.0f;
00166     }
00167     else if (output_vol_2 > 5.0f)
00168     {
00169         output_vol_2 = 5.0f;
00170     }
00171     duty_cycle_2 = (5.0f +output_vol_2) /10.0f;
00172     
00173     motor_cmd_2.write(duty_cycle_2);
00174     TIM1->CCER |= 0x40;
00175  
00176 }
00177 
00178 void CN_interrupt()
00179 {
00180     static bool hall1A_state_1;
00181     static bool hall1B_state_1;
00182     static bool hall1A_state_2;
00183     static bool hall1B_state_2;
00184     static STATE old_state_1 = DEFAULT;
00185     static STATE new_state_1 = DEFAULT;
00186     static STATE old_state_2 = DEFAULT;
00187     static STATE new_state_2 = DEFAULT;
00188 
00189 
00190     ////////////////////motor1///////////////////////
00191     hall1A_state_1 = hallA_1.read();
00192     hall1B_state_1 = hallB_1.read();
00193     old_state_1 = new_state_1;
00194     
00195     if (hall1A_state_1 == 0)
00196     {
00197         if (hall1B_state_1 == 0)
00198         {
00199             new_state_1 = ONE;
00200         }
00201         else if(hall1B_state_1 == 1)
00202         {
00203             new_state_1 = TWO;
00204         }
00205     }
00206     else if (hall1A_state_1 == 1)
00207     {
00208         if (hall1B_state_1 == 0)
00209         {
00210             new_state_1 = FOUR;
00211         }
00212         else if(hall1B_state_1 == 1)
00213         {
00214             new_state_1 = THREE;
00215         }
00216     }
00217     
00218     ///////判斷正反轉//////
00219     switch (old_state_1)
00220     {
00221         case ONE:
00222             if(new_state_1 == TWO)
00223             {
00224                 v1Count += 1;     
00225             }
00226             else if(new_state_1 == FOUR)
00227             {        
00228                 v1Count -= 1;               
00229             }
00230 
00231             break;
00232         case TWO:
00233             if(new_state_1 == THREE)
00234             {
00235                 v1Count += 1;     
00236             }
00237             else if(new_state_1 == ONE)
00238             {        
00239                 v1Count -= 1;               
00240             }
00241 
00242             break;
00243         case THREE:
00244             if(new_state_1 == FOUR)
00245             {
00246                 v1Count += 1;     
00247             }
00248             else if(new_state_1 == TWO)
00249             {        
00250                 v1Count -= 1;               
00251             }
00252 
00253             break;
00254         case FOUR:
00255             if(new_state_1 == ONE)
00256             {
00257                 v1Count += 1;     
00258             }
00259             else if(new_state_1 == THREE)
00260             {        
00261                 v1Count -= 1;               
00262             }
00263 
00264 
00265             break;
00266         case DEFAULT:
00267         
00268             break ;        
00269     }
00270     
00271     
00272     
00273     ////////////////////motor2///////////////////////
00274     hall1A_state_2 = hallA_2.read();
00275     hall1B_state_2 = hallB_2.read();
00276     old_state_2 = new_state_2;
00277     
00278     if (hall1A_state_2 == 0)
00279     {
00280         if (hall1B_state_2 == 0)
00281         {
00282             new_state_2 = ONE;
00283         }
00284         else if(hall1B_state_2 == 1)
00285         {
00286             new_state_2 = TWO;
00287         }
00288     }
00289     else if (hall1A_state_2 == 1)
00290     {
00291         if (hall1B_state_2 == 0)
00292         {
00293             new_state_2 = FOUR;
00294         }
00295         else if(hall1B_state_2 == 1)
00296         {
00297             new_state_2 = THREE;
00298         }
00299     }
00300     
00301     ///////判斷正反轉//////
00302     switch (old_state_2)
00303     {
00304         case ONE:
00305             if(new_state_2 == TWO)
00306             {
00307                 v2Count += 1;     
00308             }
00309             else if(new_state_2 == FOUR)
00310             {        
00311                 v2Count -= 1;               
00312             }
00313 
00314             break;
00315         case TWO:
00316             if(new_state_2 == THREE)
00317             {
00318                 v2Count += 1;     
00319             }
00320             else if(new_state_2 == ONE)
00321             {        
00322                 v2Count -= 1;               
00323             }
00324 
00325             break;
00326         case THREE:
00327             if(new_state_2 == FOUR)
00328             {
00329                 v2Count += 1;     
00330             }
00331             else if(new_state_2 == TWO)
00332             {        
00333                 v2Count -= 1;               
00334             }
00335 
00336             break;
00337         case FOUR:
00338             if(new_state_2 == ONE)
00339             {
00340                 v2Count += 1;     
00341             }
00342             else if(new_state_2 == THREE)
00343             {        
00344                 v2Count -= 1;               
00345             }
00346 
00347 
00348             break;
00349         case DEFAULT:
00350         
00351             break ;        
00352     }
00353 }