jiei suzuki / EC

エンコーダ・速度制御用ライブラリ

Dependents:   denku_mock_1 denku_mock_1_valve denku_mock_1_valve

Fork of EC by ROBOSTEP_SHARE

SpeedController.cpp@33:fa50666bdb34, 2017-11-17 (annotated)

Committer:
jack0325suzu
Date:
Fri Nov 17 12:59:56 2017 +0000
Revision:
33:fa50666bdb34
Parent:
32:f1c479203fa8 
123

Who changed what in which revision?

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