ロボステ6期 / PID_SC

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

PID_SC.cpp@37:c631d9b2206f, 2019-06-26 (annotated)

Committer:
aoikoizumi
Date:
Wed Jun 26 03:42:35 2019 +0000
Revision:
37:c631d9b2206f
Parent:
36:ce11c30e303c
Child:
38:1e70e21c6ce4
ereereereere

Who changed what in which revision?

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