jiei suzuki / EC

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

Dependents:   denku_mock_1 denku_mock_1_valve denku_mock_1_valve

Fork of EC by ROBOSTEP_SHARE

SpeedController.cpp@26:45a53e3c81b1, 2017-08-02 (annotated)

Committer:
jack0325suzu
Date:
Wed Aug 02 06:55:54 2017 +0000
Revision:
26:45a53e3c81b1
Parent:
24:8ac46a6d2ac4
Child:
27:72711b6cbe2a
?????????????I+PD?????I??????????????????????????I???????????????; ???????setPIDparam()?setPDparam()??????; ...

Who changed what in which revision?

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