riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.cpp@7:87c135463de7, 2016-11-19 (annotated)

Committer:
jack0325suzu
Date:
Sat Nov 19 07:03:54 2016 +0000
Revision:
7:87c135463de7
Parent:
5:4abba4f54406
Child:
9:a919aa92e65e
aaa;

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 2:a9216df32be6 8 Kv_p=0;Kv_d=0;Kv_i=0;diff=0;diff_old=0;integral=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 0:20fc96400ca3 16 double now_omega=omega;
jack0325suzu 0:20fc96400ca3 17 diff= target_omega-now_omega;
jack0325suzu 0:20fc96400ca3 18 integral+=diff;
jack0325suzu 0:20fc96400ca3 19 out_duty=Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral;
jack0325suzu 0:20fc96400ca3 20 diff_old=diff;
jack0325suzu 0:20fc96400ca3 21 if(out_duty>0.1)out_duty=0.1;
jack0325suzu 0:20fc96400ca3 22 if(out_duty<-0.1)out_duty=-0.1;
jack0325suzu 0:20fc96400ca3 23 out+=out_duty;
jack0325suzu 0:20fc96400ca3 24 duty=0.0001*out+target_omega/C;
jack0325suzu 4:1b333860dd41 25 if(duty<-0.95)duty=-0.95;
jack0325suzu 4:1b333860dd41 26 else if(duty>0.95)duty=0.95;
jack0325suzu 0:20fc96400ca3 27
jack0325suzu 0:20fc96400ca3 28 if(duty>=0){
jack0325suzu 0:20fc96400ca3 29 pwm_F_=duty;
jack0325suzu 0:20fc96400ca3 30 pwm_B_=0;
jack0325suzu 0:20fc96400ca3 31 }
jack0325suzu 0:20fc96400ca3 32 else {
jack0325suzu 0:20fc96400ca3 33 pwm_F_=0;
jack0325suzu 0:20fc96400ca3 34 pwm_B_=-duty;
jack0325suzu 0:20fc96400ca3 35 }
jack0325suzu 0:20fc96400ca3 36 }
jack0325suzu 0:20fc96400ca3 37
jack0325suzu 0:20fc96400ca3 38 void SpeedControl::turnF(double duty){
jack0325suzu 4:1b333860dd41 39 if(duty>0.95) {
jack0325suzu 4:1b333860dd41 40 pwm_F_=0.95;
jack0325suzu 4:1b333860dd41 41 pwm_B_=0;
jack0325suzu 4:1b333860dd41 42 } else {
jack0325suzu 4:1b333860dd41 43 pwm_F_=duty;
jack0325suzu 4:1b333860dd41 44 pwm_B_=0;
jack0325suzu 4:1b333860dd41 45 }
jack0325suzu 0:20fc96400ca3 46 }
jack0325suzu 0:20fc96400ca3 47
jack0325suzu 0:20fc96400ca3 48 void SpeedControl::turnB(double duty){
jack0325suzu 4:1b333860dd41 49 if(duty>0.95) {
jack0325suzu 4:1b333860dd41 50 pwm_F_=0;
jack0325suzu 4:1b333860dd41 51 pwm_B_=0.95;
jack0325suzu 4:1b333860dd41 52 } else {
jack0325suzu 4:1b333860dd41 53 pwm_F_=0;
jack0325suzu 4:1b333860dd41 54 pwm_B_=duty;
jack0325suzu 4:1b333860dd41 55 }
jack0325suzu 5:4abba4f54406 56 }
jack0325suzu 5:4abba4f54406 57
jack0325suzu 5:4abba4f54406 58 void SpeedControl::Accelarate(double target_duty){
jack0325suzu 5:4abba4f54406 59 double now_speed,old_speed;
jack0325suzu 5:4abba4f54406 60 double duty;
jack0325suzu 5:4abba4f54406 61 int start_point;
jack0325suzu 5:4abba4f54406 62 int max_point=int(target_duty/0.05);
jack0325suzu 5:4abba4f54406 63 int now_point=int((double)pwm_F_/0.05);
jack0325suzu 5:4abba4f54406 64 if(now_point<3) start_point=3;
jack0325suzu 5:4abba4f54406 65 else start_point=now_point;
jack0325suzu 5:4abba4f54406 66
jack0325suzu 5:4abba4f54406 67 if(max_point>19) max_point=19;
jack0325suzu 5:4abba4f54406 68 else if(max_point<0) max_point=0;
jack0325suzu 5:4abba4f54406 69 if(max_point>now_point){
jack0325suzu 5:4abba4f54406 70 for(int i=start_point;i<=max_point;i++){
jack0325suzu 5:4abba4f54406 71 duty=(double)i*5.0/100.0;
jack0325suzu 5:4abba4f54406 72 printf("duty = %f\r\n",(double)i*5.0/100.0);
jack0325suzu 5:4abba4f54406 73 turnF(duty);
jack0325suzu 5:4abba4f54406 74 int count=0;
jack0325suzu 5:4abba4f54406 75 while(1){
jack0325suzu 5:4abba4f54406 76 old_speed=now_speed;
jack0325suzu 5:4abba4f54406 77 wait(0.01);
jack0325suzu 5:4abba4f54406 78 now_speed=getRPM();
jack0325suzu 7:87c135463de7 79 if(now_speed<old_speed) {
jack0325suzu 7:87c135463de7 80 if(count>1) break;
jack0325suzu 5:4abba4f54406 81 else count++;
jack0325suzu 5:4abba4f54406 82 }
jack0325suzu 5:4abba4f54406 83 }
jack0325suzu 5:4abba4f54406 84 }
jack0325suzu 5:4abba4f54406 85 }
jack0325suzu 5:4abba4f54406 86 else if(max_point<now_point){
jack0325suzu 5:4abba4f54406 87 for(int i=now_point;i>=max_point;i--){
jack0325suzu 5:4abba4f54406 88 duty=(double)i*5.0/100.0;
jack0325suzu 5:4abba4f54406 89 printf("duty = %f\r\n",(double)i*5.0/100.0);
jack0325suzu 5:4abba4f54406 90 turnF(duty);
jack0325suzu 5:4abba4f54406 91 int count=0;
jack0325suzu 5:4abba4f54406 92 while(1){
jack0325suzu 5:4abba4f54406 93 old_speed=now_speed;
jack0325suzu 5:4abba4f54406 94 wait(0.01);
jack0325suzu 5:4abba4f54406 95 now_speed=getRPM();
jack0325suzu 7:87c135463de7 96 if(now_speed>old_speed) {
jack0325suzu 7:87c135463de7 97 if(count>1) break;
jack0325suzu 5:4abba4f54406 98 else count++;
jack0325suzu 5:4abba4f54406 99 }
jack0325suzu 5:4abba4f54406 100 }
jack0325suzu 5:4abba4f54406 101 }
jack0325suzu 5:4abba4f54406 102 }
jack0325suzu 5:4abba4f54406 103 }
jack0325suzu 5:4abba4f54406 104
jack0325suzu 5:4abba4f54406 105 void SpeedControl::ScZ(double target_RPM){
jack0325suzu 5:4abba4f54406 106 double now_RPM=getRPM();
jack0325suzu 5:4abba4f54406 107 if(fabs(now_RPM-target_RPM)>200) Accelarate(target_RPM/C);
jack0325suzu 5:4abba4f54406 108 diff=target_RPM-now_RPM;
jack0325suzu 5:4abba4f54406 109 integral+=diff;
jack0325suzu 5:4abba4f54406 110 out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral);
jack0325suzu 5:4abba4f54406 111 diff_old=diff;
jack0325suzu 5:4abba4f54406 112 if(out_duty>0.01)out_duty=0.01;
jack0325suzu 5:4abba4f54406 113 if(out_duty<-0.01)out_duty=-0.01;
jack0325suzu 5:4abba4f54406 114 out+=out_duty;
jack0325suzu 5:4abba4f54406 115 duty=0.0001*out+target_RPM/C;
jack0325suzu 5:4abba4f54406 116 turnF(duty);
jack0325suzu 5:4abba4f54406 117 }
jack0325suzu 5:4abba4f54406 118
jack0325suzu 5:4abba4f54406 119
jack0325suzu 5:4abba4f54406 120 void SpeedControl::setPIDparam(double p,double i,double d){
jack0325suzu 5:4abba4f54406 121 Kv_p=p;
jack0325suzu 5:4abba4f54406 122 Kv_d=d;
jack0325suzu 5:4abba4f54406 123 Kv_i=i;
jack0325suzu 5:4abba4f54406 124 }
jack0325suzu 5:4abba4f54406 125
jack0325suzu 5:4abba4f54406 126
jack0325suzu 5:4abba4f54406 127 void SpeedControl::setDOconstant(double c){
jack0325suzu 5:4abba4f54406 128 C=c;
jack0325suzu 5:4abba4f54406 129 }
jack0325suzu 5:4abba4f54406 130
jack0325suzu 5:4abba4f54406 131 void SpeedControl::stop(){
jack0325suzu 5:4abba4f54406 132 pwm_F_=0;
jack0325suzu 5:4abba4f54406 133 pwm_B_=0;
jack0325suzu 5:4abba4f54406 134 }
jack0325suzu 5:4abba4f54406 135