riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.cpp@16:14d05ae8d566, 2017-04-10 (annotated)

Committer:
jack0325suzu
Date:
Mon Apr 10 10:14:38 2017 +0000
Revision:
16:14d05ae8d566
Parent:
15:aef7cc059e50
Child:
17:e0b465d9b579
qqq

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 10:216d5a573dc7 8 Kv_p=0;Kv_d=0;Kv_i=0;diff=0;diff_old=0;integral=0;now_time_=0;old_time_=0;
jack0325suzu 0:20fc96400ca3 9 out_duty=0;out=0;duty=0;
jack0325suzu 0:20fc96400ca3 10 pwm_F_.period_us(100);
jack0325suzu 0:20fc96400ca3 11 pwm_B_.period_us(100);
jack0325suzu 0:20fc96400ca3 12 C=45.0;
jack0325suzu 0:20fc96400ca3 13 }
jack0325suzu 0:20fc96400ca3 14
jack0325suzu 0:20fc96400ca3 15 void SpeedControl::Sc(double target_omega){ //スカンジウムじゃないよ
jack0325suzu 10:216d5a573dc7 16 now_omega=omega;
jack0325suzu 10:216d5a573dc7 17 now_time_=timer.read();
jack0325suzu 0:20fc96400ca3 18 diff= target_omega-now_omega;
jack0325suzu 0:20fc96400ca3 19 integral+=diff;
jack0325suzu 10:216d5a573dc7 20 out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral*(now_time_-old_time_);
jack0325suzu 0:20fc96400ca3 21 diff_old=diff;
jack0325suzu 0:20fc96400ca3 22 if(out_duty>0.1)out_duty=0.1;
jack0325suzu 0:20fc96400ca3 23 if(out_duty<-0.1)out_duty=-0.1;
jack0325suzu 0:20fc96400ca3 24 out+=out_duty;
jack0325suzu 0:20fc96400ca3 25 duty=0.0001*out+target_omega/C;
jack0325suzu 4:1b333860dd41 26 if(duty<-0.95)duty=-0.95;
jack0325suzu 4:1b333860dd41 27 else if(duty>0.95)duty=0.95;
jack0325suzu 10:216d5a573dc7 28 old_time_=now_time_;
jack0325suzu 0:20fc96400ca3 29
jack0325suzu 0:20fc96400ca3 30 if(duty>=0){
jack0325suzu 0:20fc96400ca3 31 pwm_F_=duty;
jack0325suzu 0:20fc96400ca3 32 pwm_B_=0;
jack0325suzu 0:20fc96400ca3 33 }
jack0325suzu 0:20fc96400ca3 34 else {
jack0325suzu 0:20fc96400ca3 35 pwm_F_=0;
jack0325suzu 0:20fc96400ca3 36 pwm_B_=-duty;
jack0325suzu 0:20fc96400ca3 37 }
jack0325suzu 0:20fc96400ca3 38 }
jack0325suzu 0:20fc96400ca3 39
jack0325suzu 0:20fc96400ca3 40 void SpeedControl::turnF(double duty){
jack0325suzu 4:1b333860dd41 41 if(duty>0.95) {
jack0325suzu 4:1b333860dd41 42 pwm_F_=0.95;
jack0325suzu 4:1b333860dd41 43 pwm_B_=0;
jack0325suzu 4:1b333860dd41 44 } else {
jack0325suzu 4:1b333860dd41 45 pwm_F_=duty;
jack0325suzu 4:1b333860dd41 46 pwm_B_=0;
jack0325suzu 4:1b333860dd41 47 }
jack0325suzu 0:20fc96400ca3 48 }
jack0325suzu 0:20fc96400ca3 49
jack0325suzu 0:20fc96400ca3 50 void SpeedControl::turnB(double duty){
jack0325suzu 4:1b333860dd41 51 if(duty>0.95) {
jack0325suzu 4:1b333860dd41 52 pwm_F_=0;
jack0325suzu 4:1b333860dd41 53 pwm_B_=0.95;
jack0325suzu 4:1b333860dd41 54 } else {
jack0325suzu 4:1b333860dd41 55 pwm_F_=0;
jack0325suzu 4:1b333860dd41 56 pwm_B_=duty;
jack0325suzu 4:1b333860dd41 57 }
jack0325suzu 5:4abba4f54406 58 }
jack0325suzu 5:4abba4f54406 59
jack0325suzu 5:4abba4f54406 60 void SpeedControl::Accelarate(double target_duty){
jack0325suzu 5:4abba4f54406 61 double now_speed,old_speed;
jack0325suzu 5:4abba4f54406 62 double duty;
jack0325suzu 5:4abba4f54406 63 int start_point;
jack0325suzu 5:4abba4f54406 64 int max_point=int(target_duty/0.05);
jack0325suzu 5:4abba4f54406 65 int now_point=int((double)pwm_F_/0.05);
jack0325suzu 5:4abba4f54406 66 if(now_point<3) start_point=3;
jack0325suzu 5:4abba4f54406 67 else start_point=now_point;
jack0325suzu 5:4abba4f54406 68
jack0325suzu 5:4abba4f54406 69 if(max_point>19) max_point=19;
jack0325suzu 5:4abba4f54406 70 else if(max_point<0) max_point=0;
jack0325suzu 5:4abba4f54406 71 if(max_point>now_point){
jack0325suzu 5:4abba4f54406 72 for(int i=start_point;i<=max_point;i++){
jack0325suzu 5:4abba4f54406 73 duty=(double)i*5.0/100.0;
jack0325suzu 9:a919aa92e65e 74 printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
jack0325suzu 5:4abba4f54406 75 turnF(duty);
jack0325suzu 5:4abba4f54406 76 int count=0;
jack0325suzu 5:4abba4f54406 77 while(1){
jack0325suzu 5:4abba4f54406 78 old_speed=now_speed;
jack0325suzu 5:4abba4f54406 79 wait(0.01);
jack0325suzu 5:4abba4f54406 80 now_speed=getRPM();
jack0325suzu 7:87c135463de7 81 if(now_speed<old_speed) {
jack0325suzu 7:87c135463de7 82 if(count>1) break;
jack0325suzu 5:4abba4f54406 83 else count++;
jack0325suzu 5:4abba4f54406 84 }
jack0325suzu 5:4abba4f54406 85 }
jack0325suzu 5:4abba4f54406 86 }
jack0325suzu 5:4abba4f54406 87 }
jack0325suzu 5:4abba4f54406 88 else if(max_point<now_point){
jack0325suzu 5:4abba4f54406 89 for(int i=now_point;i>=max_point;i--){
jack0325suzu 5:4abba4f54406 90 duty=(double)i*5.0/100.0;
jack0325suzu 9:a919aa92e65e 91 printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
jack0325suzu 5:4abba4f54406 92 turnF(duty);
jack0325suzu 5:4abba4f54406 93 int count=0;
jack0325suzu 5:4abba4f54406 94 while(1){
jack0325suzu 5:4abba4f54406 95 old_speed=now_speed;
jack0325suzu 5:4abba4f54406 96 wait(0.01);
jack0325suzu 5:4abba4f54406 97 now_speed=getRPM();
jack0325suzu 7:87c135463de7 98 if(now_speed>old_speed) {
jack0325suzu 7:87c135463de7 99 if(count>1) break;
jack0325suzu 5:4abba4f54406 100 else count++;
jack0325suzu 5:4abba4f54406 101 }
jack0325suzu 5:4abba4f54406 102 }
jack0325suzu 5:4abba4f54406 103 }
jack0325suzu 5:4abba4f54406 104 }
jack0325suzu 5:4abba4f54406 105 }
jack0325suzu 5:4abba4f54406 106
jack0325suzu 5:4abba4f54406 107 void SpeedControl::ScZ(double target_RPM){
jack0325suzu 9:a919aa92e65e 108 now_RPM=getRPM();
jack0325suzu 15:aef7cc059e50 109 //if(fabs(now_RPM-target_RPM)>300) Accelarate(target_RPM/C);
jack0325suzu 5:4abba4f54406 110 diff=target_RPM-now_RPM;
jack0325suzu 5:4abba4f54406 111 integral+=diff;
jack0325suzu 5:4abba4f54406 112 out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral);
jack0325suzu 5:4abba4f54406 113 diff_old=diff;
jack0325suzu 5:4abba4f54406 114 if(out_duty>0.01)out_duty=0.01;
jack0325suzu 5:4abba4f54406 115 if(out_duty<-0.01)out_duty=-0.01;
jack0325suzu 5:4abba4f54406 116 out+=out_duty;
jack0325suzu 16:14d05ae8d566 117 duty=0.0001*out/*+target_RPM/C*/;
jack0325suzu 5:4abba4f54406 118 turnF(duty);
jack0325suzu 5:4abba4f54406 119 }
jack0325suzu 9:a919aa92e65e 120
jack0325suzu 9:a919aa92e65e 121 void SpeedControl::ScZ2(double target_RPM){
jack0325suzu 12:530f6184830a 122 now_time_=timer.read();
jack0325suzu 9:a919aa92e65e 123 now_RPM=getRPM();
jack0325suzu 9:a919aa92e65e 124 diff=target_RPM-now_RPM;
jack0325suzu 9:a919aa92e65e 125 integral+=diff;
jack0325suzu 13:caa6db604077 126 out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral;
jack0325suzu 12:530f6184830a 127 old_time_=now_time_;
jack0325suzu 9:a919aa92e65e 128 diff_old=diff;
jack0325suzu 9:a919aa92e65e 129 if(out_duty>0.001)out_duty=0.001;
jack0325suzu 9:a919aa92e65e 130 if(out_duty<-0.001)out_duty=-0.001;
jack0325suzu 9:a919aa92e65e 131 out+=out_duty;
jack0325suzu 12:530f6184830a 132 duty=0.001*out;
jack0325suzu 9:a919aa92e65e 133 turnF(duty);
jack0325suzu 9:a919aa92e65e 134 }
jack0325suzu 5:4abba4f54406 135
jack0325suzu 5:4abba4f54406 136
jack0325suzu 5:4abba4f54406 137 void SpeedControl::setPIDparam(double p,double i,double d){
jack0325suzu 5:4abba4f54406 138 Kv_p=p;
jack0325suzu 5:4abba4f54406 139 Kv_d=d;
jack0325suzu 5:4abba4f54406 140 Kv_i=i;
jack0325suzu 5:4abba4f54406 141 }
jack0325suzu 5:4abba4f54406 142
jack0325suzu 5:4abba4f54406 143
jack0325suzu 5:4abba4f54406 144 void SpeedControl::setDOconstant(double c){
jack0325suzu 5:4abba4f54406 145 C=c;
jack0325suzu 5:4abba4f54406 146 }
jack0325suzu 5:4abba4f54406 147
jack0325suzu 12:530f6184830a 148 void SpeedControl::reset(){
jack0325suzu 12:530f6184830a 149 S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
jack0325suzu 12:530f6184830a 150 rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
jack0325suzu 12:530f6184830a 151 diff=0;diff_old=0;integral=0;now_time_=0;old_time_=0;
jack0325suzu 12:530f6184830a 152 out=0;out_duty=0;
jack0325suzu 12:530f6184830a 153 }
jack0325suzu 12:530f6184830a 154
jack0325suzu 5:4abba4f54406 155 void SpeedControl::stop(){
jack0325suzu 5:4abba4f54406 156 pwm_F_=0;
jack0325suzu 5:4abba4f54406 157 pwm_B_=0;
jack0325suzu 5:4abba4f54406 158 }
jack0325suzu 5:4abba4f54406 159