SpeedController.cpp

00001 
00002 #include "mbed.h"
00003 #include "SpeedController.h"
00004 
00005 /***EC classの方を先に読んでおくこと***/
00006 
00007 
00008 SpeedControl::SpeedControl(PinName signalA , PinName signalB , PinName signalZ , int s , double t , PinName pwm_F , PinName pwm_B) : Ec(signalA,signalB,signalZ,s,t) , pwm_F_(pwm_F),pwm_B_(pwm_B) {
00009     
00010     Kv_p=0;Kv_d=0;diff=0;diff_old=0;now_time_=0;old_time_=0;
00011     now_omega=0;now_RPM=0;
00012     out_duty=0;out=0;duty=0;
00013     pwm_F_.period_us(100);
00014     pwm_B_.period_us(100);
00015     C=45.0;
00016 }
00017 
00018 void SpeedControl::Sc(double target_omega){       //スカンジウムじゃないよ
00019     now_omega=omega;
00020     now_time_=timer.read();
00021     diff= (target_omega-now_omega)/C;
00022     out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
00023     diff_old=diff;
00024 
00025     if(out_duty>0.1)out_duty=0.1;
00026     if(out_duty<-0.1)out_duty=-0.1;
00027 
00028     if((duty>=-0.95)&&(duty<=0.95)) duty=duty+out_duty;
00029     //duty=target_omega/C;
00030     if(duty<-0.95)duty=-0.95;
00031     else if(duty>0.95)duty=0.95;
00032     old_time_=now_time_;
00033     
00034     if(duty>=0){
00035         pwm_F_=duty;
00036         pwm_B_=0;
00037     }
00038     else {
00039         pwm_F_=0;
00040         pwm_B_=-duty;
00041     }
00042 }
00043 
00044 void SpeedControl::turnF(double duty){
00045     if(duty>0.95) {
00046         pwm_F_=0.95;
00047         pwm_B_=0;
00048     } else if(duty<0) {
00049         pwm_F_=0;
00050         pwm_B_=0;
00051     } else {
00052         pwm_F_=duty;
00053         pwm_B_=0;
00054     }
00055 }
00056 
00057 void SpeedControl::turnB(double duty){
00058     if(duty>0.95) {
00059         pwm_F_=0;
00060         pwm_B_=0.95;
00061     } else if(duty<0) {
00062         pwm_F_=0;
00063         pwm_B_=0;
00064     } else {
00065         pwm_F_=0;
00066         pwm_B_=duty;
00067     }
00068 }
00069 
00070 //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
00071 /*void SpeedControl::Accelarate(double target_duty){
00072     double now_speed,old_speed;
00073     double duty;
00074     int start_point;
00075     int max_point=int(target_duty/0.05);
00076     int now_point=int((double)pwm_F_/0.05);
00077     if(now_point<3) start_point=3;
00078     else start_point=now_point;
00079     
00080     if(max_point>19) max_point=19;
00081     else if(max_point<0) max_point=0;
00082     if(max_point>now_point){
00083         for(int i=start_point;i<=max_point;i++){
00084             duty=(double)i*5.0/100.0;
00085             printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
00086             turnF(duty);
00087             int count=0;
00088             while(1){
00089                 old_speed=now_speed;
00090                 wait(0.01);
00091                 now_speed=getRPM();
00092                 if(now_speed<old_speed) {
00093                     if(count>1) break;
00094                     else count++;
00095                 }
00096             }
00097         }
00098     }
00099     else if(max_point<now_point){
00100         for(int i=now_point;i>=max_point;i--){
00101             duty=(double)i*5.0/100.0;
00102             printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
00103             turnF(duty);
00104             int count=0;
00105             while(1){
00106                 old_speed=now_speed;
00107                 wait(0.01);
00108                 now_speed=getRPM();
00109                 if(now_speed>old_speed) {
00110                     if(count>1) break;
00111                     else count++;
00112                 }
00113             }
00114         }
00115     }
00116 }*/
00117 
00118 /*void SpeedControl::ScZ(double target_RPM){
00119     now_RPM=getRPM();
00120     //if(fabs(now_RPM-target_RPM)>300) Accelarate(target_RPM/C);
00121     diff=target_RPM-now_RPM;
00122     integral+=diff;
00123     out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old));
00124     diff_old=diff;
00125     if(out_duty>0.04)out_duty=0.04;
00126     if(out_duty<-0.04)out_duty=-0.04;
00127     if((duty>=0)&&(duty<0.95)) out+=out_duty;
00128     duty=0.0001*out+target_RPM/C;
00129     if(duty>=0.95) {
00130         duty=0.94;
00131         out=0.94/0.0001;
00132     }
00133     turnF(duty);
00134 }*/
00135 
00136 void SpeedControl::ScZ2(double target_RPM){
00137     now_time_=timer.read();
00138     now_RPM=getRPM();
00139     diff=target_RPM-now_RPM;
00140     out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
00141     old_time_=now_time_;
00142     diff_old=diff;
00143     if(out_duty>0.001)out_duty=0.001;
00144     if(out_duty<-0.001)out_duty=-0.001;
00145     if((duty>0)&&(duty<0.95)) out+=out_duty;
00146     duty=0.001*out;
00147     turnF(duty);
00148 }
00149     
00150 
00151 void SpeedControl::setPDparam(double p,double d){
00152     Kv_p=p;
00153     Kv_d=d;
00154 }
00155 
00156 
00157 void SpeedControl::setDOconstant(double c){
00158     C=c;
00159 }
00160 
00161 void SpeedControl::reset(){
00162     S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
00163     rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
00164     diff=0;diff_old=0;now_time_=0;old_time_=0;
00165     out=0;out_duty=0;
00166 }
00167 
00168 void SpeedControl::stop(){
00169     pwm_F_=0;
00170     pwm_B_=0;
00171 }