riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.cpp@4:1b333860dd41, 2016-11-02 (annotated)

Committer:
jack0325suzu
Date:
Wed Nov 02 02:43:15 2016 +0000
Revision:
4:1b333860dd41
Parent:
2:a9216df32be6
Child:
5:4abba4f54406
??????????????

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 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 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
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 4:1b333860dd41 56 if(duty>0.95) {
jack0325suzu 4:1b333860dd41 57 pwm_F_=0.95;
jack0325suzu 4:1b333860dd41 58 pwm_B_=0;
jack0325suzu 4:1b333860dd41 59 } else {
jack0325suzu 4:1b333860dd41 60 pwm_F_=duty;
jack0325suzu 4:1b333860dd41 61 pwm_B_=0;
jack0325suzu 4:1b333860dd41 62 }
jack0325suzu 0:20fc96400ca3 63 }
jack0325suzu 0:20fc96400ca3 64
jack0325suzu 0:20fc96400ca3 65 void SpeedControl::turnB(double duty){
jack0325suzu 4:1b333860dd41 66 if(duty>0.95) {
jack0325suzu 4:1b333860dd41 67 pwm_F_=0;
jack0325suzu 4:1b333860dd41 68 pwm_B_=0.95;
jack0325suzu 4:1b333860dd41 69 } else {
jack0325suzu 4:1b333860dd41 70 pwm_F_=0;
jack0325suzu 4:1b333860dd41 71 pwm_B_=duty;
jack0325suzu 4:1b333860dd41 72 }
jack0325suzu 0:20fc96400ca3 73 }