7th_DENSOU / Mbed 2 deprecated nhk_2021motorAB_

motorAB

Dependencies:   mbed ros_lib_melodic

EC/SpeedController.cpp@0:cca61e773cbb, 2020-04-03 (annotated)

Committer:
la00noix
Date:
Fri Apr 03 02:52:21 2020 +0000
Revision:
0:cca61e773cbb
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:cca61e773cbb 1
la00noix 0:cca61e773cbb 2 #include "mbed.h"
la00noix 0:cca61e773cbb 3 #include "EC.h"
la00noix 0:cca61e773cbb 4
la00noix 0:cca61e773cbb 5 /***EC classの方を先に読んでおくこと***/
la00noix 0:cca61e773cbb 6
la00noix 0:cca61e773cbb 7
la00noix 0:cca61e773cbb 8 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) {
la00noix 0:cca61e773cbb 9
la00noix 0:cca61e773cbb 10 Kv_p=0;Kv_d=0;diff=0;diff_old=0;now_time_=0;old_time_=0;
la00noix 0:cca61e773cbb 11 now_omega=0;now_RPM=0;
la00noix 0:cca61e773cbb 12 out_duty=0;out_plus=0;out_minus=0;duty=0;sth_debug=0;
la00noix 0:cca61e773cbb 13 pwm_F_.period_us(50);
la00noix 0:cca61e773cbb 14 pwm_B_.period_us(50);
la00noix 0:cca61e773cbb 15 Cf=45.0;
la00noix 0:cca61e773cbb 16 Df=0;
la00noix 0:cca61e773cbb 17 Cb=45.0;
la00noix 0:cca61e773cbb 18 Db=0;
la00noix 0:cca61e773cbb 19 }
la00noix 0:cca61e773cbb 20
la00noix 0:cca61e773cbb 21 void SpeedControl::Sc(double target_omega){ //スカンジウムじゃないよ
la00noix 0:cca61e773cbb 22 now_omega=omega;
la00noix 0:cca61e773cbb 23 now_time_=timer.read();
la00noix 0:cca61e773cbb 24 diff= target_omega-now_omega;
la00noix 0:cca61e773cbb 25 out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
la00noix 0:cca61e773cbb 26 diff_old=diff;
la00noix 0:cca61e773cbb 27 if(out_duty>0.1)out_duty=0.1;
la00noix 0:cca61e773cbb 28 if(out_duty<-0.1)out_duty=-0.1;
la00noix 0:cca61e773cbb 29 if(target_omega>0){
la00noix 0:cca61e773cbb 30 if((duty>=-0.8)&&(duty<=0.8)) out_plus+=out_duty;
la00noix 0:cca61e773cbb 31 duty=0.00002*out_plus+target_omega*Cf+Df;
la00noix 0:cca61e773cbb 32 if(duty<0)duty=0;
la00noix 0:cca61e773cbb 33 }else{
la00noix 0:cca61e773cbb 34 if((duty>=-0.8)&&(duty<=0.8)) out_minus+=out_duty;
la00noix 0:cca61e773cbb 35 duty=0.00002*out_minus+target_omega*Cb-Db;
la00noix 0:cca61e773cbb 36 if(duty>0)duty=0;
la00noix 0:cca61e773cbb 37 }
la00noix 0:cca61e773cbb 38 if(duty<-0.8)duty=-0.8;
la00noix 0:cca61e773cbb 39 else if(duty>0.8)duty=0.8;
la00noix 0:cca61e773cbb 40 old_time_=now_time_;
la00noix 0:cca61e773cbb 41
la00noix 0:cca61e773cbb 42 if(duty>=0){
la00noix 0:cca61e773cbb 43 pwm_F_=duty;
la00noix 0:cca61e773cbb 44 pwm_B_=0;
la00noix 0:cca61e773cbb 45 } else {
la00noix 0:cca61e773cbb 46 pwm_F_=0;
la00noix 0:cca61e773cbb 47 pwm_B_=-duty;
la00noix 0:cca61e773cbb 48 }
la00noix 0:cca61e773cbb 49 }
la00noix 0:cca61e773cbb 50
la00noix 0:cca61e773cbb 51 void SpeedControl::turnF(double duty){
la00noix 0:cca61e773cbb 52 if(duty>0.95) {
la00noix 0:cca61e773cbb 53 pwm_F_=0.95;
la00noix 0:cca61e773cbb 54 pwm_B_=0;
la00noix 0:cca61e773cbb 55 } else if(duty<0) {
la00noix 0:cca61e773cbb 56 pwm_F_=0;
la00noix 0:cca61e773cbb 57 pwm_B_=0;
la00noix 0:cca61e773cbb 58 } else {
la00noix 0:cca61e773cbb 59 pwm_F_=duty;
la00noix 0:cca61e773cbb 60 pwm_B_=0;
la00noix 0:cca61e773cbb 61 }
la00noix 0:cca61e773cbb 62 }
la00noix 0:cca61e773cbb 63
la00noix 0:cca61e773cbb 64 void SpeedControl::turnB(double duty){
la00noix 0:cca61e773cbb 65 if(duty>0.95) {
la00noix 0:cca61e773cbb 66 pwm_F_=0;
la00noix 0:cca61e773cbb 67 pwm_B_=0.95;
la00noix 0:cca61e773cbb 68 } else if(duty<0) {
la00noix 0:cca61e773cbb 69 pwm_F_=0;
la00noix 0:cca61e773cbb 70 pwm_B_=0;
la00noix 0:cca61e773cbb 71 } else {
la00noix 0:cca61e773cbb 72 pwm_F_=0;
la00noix 0:cca61e773cbb 73 pwm_B_=duty;
la00noix 0:cca61e773cbb 74 }
la00noix 0:cca61e773cbb 75 }
la00noix 0:cca61e773cbb 76
la00noix 0:cca61e773cbb 77 //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
la00noix 0:cca61e773cbb 78 /*void SpeedControl::Accelarate(double target_duty){
la00noix 0:cca61e773cbb 79 double now_speed,old_speed;
la00noix 0:cca61e773cbb 80 double duty;
la00noix 0:cca61e773cbb 81 int start_point;
la00noix 0:cca61e773cbb 82 int max_point=int(target_duty/0.05);
la00noix 0:cca61e773cbb 83 int now_point=int((double)pwm_F_/0.05);
la00noix 0:cca61e773cbb 84 if(now_point<3) start_point=3;
la00noix 0:cca61e773cbb 85 else start_point=now_point;
la00noix 0:cca61e773cbb 86
la00noix 0:cca61e773cbb 87 if(max_point>19) max_point=19;
la00noix 0:cca61e773cbb 88 else if(max_point<0) max_point=0;
la00noix 0:cca61e773cbb 89 if(max_point>now_point){
la00noix 0:cca61e773cbb 90 for(int i=start_point;i<=max_point;i++){
la00noix 0:cca61e773cbb 91 duty=(double)i*5.0/100.0;
la00noix 0:cca61e773cbb 92 printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
la00noix 0:cca61e773cbb 93 turnF(duty);
la00noix 0:cca61e773cbb 94 int count=0;
la00noix 0:cca61e773cbb 95 while(1){
la00noix 0:cca61e773cbb 96 old_speed=now_speed;
la00noix 0:cca61e773cbb 97 wait(0.01);
la00noix 0:cca61e773cbb 98 now_speed=getRPM();
la00noix 0:cca61e773cbb 99 if(now_speed<old_speed) {
la00noix 0:cca61e773cbb 100 if(count>1) break;
la00noix 0:cca61e773cbb 101 else count++;
la00noix 0:cca61e773cbb 102 }
la00noix 0:cca61e773cbb 103 }
la00noix 0:cca61e773cbb 104 }
la00noix 0:cca61e773cbb 105 }
la00noix 0:cca61e773cbb 106 else if(max_point<now_point){
la00noix 0:cca61e773cbb 107 for(int i=now_point;i>=max_point;i--){
la00noix 0:cca61e773cbb 108 duty=(double)i*5.0/100.0;
la00noix 0:cca61e773cbb 109 printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
la00noix 0:cca61e773cbb 110 turnF(duty);
la00noix 0:cca61e773cbb 111 int count=0;
la00noix 0:cca61e773cbb 112 while(1){
la00noix 0:cca61e773cbb 113 old_speed=now_speed;
la00noix 0:cca61e773cbb 114 wait(0.01);
la00noix 0:cca61e773cbb 115 now_speed=getRPM();
la00noix 0:cca61e773cbb 116 if(now_speed>old_speed) {
la00noix 0:cca61e773cbb 117 if(count>1) break;
la00noix 0:cca61e773cbb 118 else count++;
la00noix 0:cca61e773cbb 119 }
la00noix 0:cca61e773cbb 120 }
la00noix 0:cca61e773cbb 121 }
la00noix 0:cca61e773cbb 122 }
la00noix 0:cca61e773cbb 123 }*/
la00noix 0:cca61e773cbb 124
la00noix 0:cca61e773cbb 125 /*void SpeedControl::ScZ(double target_RPM){
la00noix 0:cca61e773cbb 126 now_RPM=getRPM();
la00noix 0:cca61e773cbb 127 //if(fabs(now_RPM-target_RPM)>300) Accelarate(target_RPM/C);
la00noix 0:cca61e773cbb 128 diff=target_RPM-now_RPM;
la00noix 0:cca61e773cbb 129 integral+=diff;
la00noix 0:cca61e773cbb 130 out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old));
la00noix 0:cca61e773cbb 131 diff_old=diff;
la00noix 0:cca61e773cbb 132 if(out_duty>0.04)out_duty=0.04;
la00noix 0:cca61e773cbb 133 if(out_duty<-0.04)out_duty=-0.04;
la00noix 0:cca61e773cbb 134 if((duty>=0)&&(duty<0.95)) out+=out_duty;
la00noix 0:cca61e773cbb 135 duty=0.0001*out+target_RPM/C;
la00noix 0:cca61e773cbb 136 if(duty>=0.95) {
la00noix 0:cca61e773cbb 137 duty=0.94;
la00noix 0:cca61e773cbb 138 out=0.94/0.0001;
la00noix 0:cca61e773cbb 139 }
la00noix 0:cca61e773cbb 140 turnF(duty);
la00noix 0:cca61e773cbb 141 }*/
la00noix 0:cca61e773cbb 142
la00noix 0:cca61e773cbb 143 /*void SpeedControl::ScZ2(double target_RPM){
la00noix 0:cca61e773cbb 144 now_time_=timer.read();
la00noix 0:cca61e773cbb 145 now_RPM=getRPM();
la00noix 0:cca61e773cbb 146 diff=target_RPM-now_RPM;
la00noix 0:cca61e773cbb 147 out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
la00noix 0:cca61e773cbb 148 old_time_=now_time_;
la00noix 0:cca61e773cbb 149 diff_old=diff;
la00noix 0:cca61e773cbb 150 if(out_duty>0.001)out_duty=0.001;
la00noix 0:cca61e773cbb 151 if(out_duty<-0.001)out_duty=-0.001;
la00noix 0:cca61e773cbb 152 if((duty>0)&&(duty<0.95)) out+=out_duty;
la00noix 0:cca61e773cbb 153 duty=0.001*out;
la00noix 0:cca61e773cbb 154 turnF(duty);
la00noix 0:cca61e773cbb 155 }*/
la00noix 0:cca61e773cbb 156
la00noix 0:cca61e773cbb 157
la00noix 0:cca61e773cbb 158 void SpeedControl::setPDparam(double p,double d){
la00noix 0:cca61e773cbb 159 Kv_p=p;
la00noix 0:cca61e773cbb 160 Kv_d=d;
la00noix 0:cca61e773cbb 161 }
la00noix 0:cca61e773cbb 162
la00noix 0:cca61e773cbb 163
la00noix 0:cca61e773cbb 164 void SpeedControl::setDOconstant(double cf,double df,double cb,double db){
la00noix 0:cca61e773cbb 165 Cf=1/cf;
la00noix 0:cca61e773cbb 166 Df=df;
la00noix 0:cca61e773cbb 167 Cb=1/cb;
la00noix 0:cca61e773cbb 168 Db=db;
la00noix 0:cca61e773cbb 169 }
la00noix 0:cca61e773cbb 170
la00noix 0:cca61e773cbb 171 void SpeedControl::reset(){
la00noix 0:cca61e773cbb 172 S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
la00noix 0:cca61e773cbb 173 rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
la00noix 0:cca61e773cbb 174 diff=0;diff_old=0;now_time_=0;old_time_=0;
la00noix 0:cca61e773cbb 175 out_plus=0;out_minus=0;out_duty=0;duty=0;
la00noix 0:cca61e773cbb 176 timer.stop();
la00noix 0:cca61e773cbb 177 timer.reset();
la00noix 0:cca61e773cbb 178 timer.start();
la00noix 0:cca61e773cbb 179 }
la00noix 0:cca61e773cbb 180
la00noix 0:cca61e773cbb 181 void SpeedControl::stop(){
la00noix 0:cca61e773cbb 182 pwm_F_=0;
la00noix 0:cca61e773cbb 183 pwm_B_=0;
la00noix 0:cca61e773cbb 184 }