riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.cpp@32:297384f9d261, 2018-07-04 (annotated)

Committer:
shimizuta
Date:
Wed Jul 04 04:21:37 2018 +0000
Revision:
32:297384f9d261
Parent:
29:b8de333facd4
Child:
35:b47330f0dec2
add getDistance_mm()

Who changed what in which revision?

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