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