riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.cpp@14:28b6628fe90f, 2017-04-10 (annotated)

Committer:
jack0325suzu
Date:
Mon Apr 10 08:10:33 2017 +0000
Revision:
14:28b6628fe90f
Parent:
13:caa6db604077 
ScZ????????

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