riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.cpp@0:20fc96400ca3, 2016-06-16 (annotated)

Committer:
jack0325suzu
Date:
Thu Jun 16 07:36:15 2016 +0000
Revision:
0:20fc96400ca3
Child:
2:a9216df32be6
????????????????

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 0:20fc96400ca3 4 /***ECライブラリの方を先に読んでおくこと***/
jack0325suzu 0:20fc96400ca3 5
jack0325suzu 0:20fc96400ca3 6
jack0325suzu 0:20fc96400ca3 7 SpeedControl::SpeedControl(PinName signalA , PinName signalB , int s , double t , PinName pwm_F , PinName pwm_B) : Ec(signalA,signalB,s,t) , pwm_F_(pwm_F),pwm_B_(pwm_B) {
jack0325suzu 0:20fc96400ca3 8 Kv_p=0;Kv_p=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 0:20fc96400ca3 25 if(duty<-0.5)duty=-0.5;
jack0325suzu 0:20fc96400ca3 26 else if(duty>0.5)duty=0.5;
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
jack0325suzu 0:20fc96400ca3 39 void SpeedControl::setPIDparam(double p,double i,double d){
jack0325suzu 0:20fc96400ca3 40 Kv_p=p;
jack0325suzu 0:20fc96400ca3 41 Kv_d=d;
jack0325suzu 0:20fc96400ca3 42 Kv_i=i;
jack0325suzu 0:20fc96400ca3 43 }
jack0325suzu 0:20fc96400ca3 44
jack0325suzu 0:20fc96400ca3 45
jack0325suzu 0:20fc96400ca3 46 void SpeedControl::setDOconstant(double c){
jack0325suzu 0:20fc96400ca3 47 C=c;
jack0325suzu 0:20fc96400ca3 48 }
jack0325suzu 0:20fc96400ca3 49
jack0325suzu 0:20fc96400ca3 50 void SpeedControl::stop(){
jack0325suzu 0:20fc96400ca3 51 pwm_F_=0;
jack0325suzu 0:20fc96400ca3 52 pwm_B_=0;
jack0325suzu 0:20fc96400ca3 53 }
jack0325suzu 0:20fc96400ca3 54
jack0325suzu 0:20fc96400ca3 55 void SpeedControl::turnF(double duty){
jack0325suzu 0:20fc96400ca3 56 pwm_F_=duty;
jack0325suzu 0:20fc96400ca3 57 pwm_B_=0;
jack0325suzu 0:20fc96400ca3 58 }
jack0325suzu 0:20fc96400ca3 59
jack0325suzu 0:20fc96400ca3 60 void SpeedControl::turnB(double duty){
jack0325suzu 0:20fc96400ca3 61 pwm_F_=0;
jack0325suzu 0:20fc96400ca3 62 pwm_B_=duty;
jack0325suzu 0:20fc96400ca3 63 }