Diff: SpeedController.cpp

Revision:

0:20fc96400ca3

Child:

2:a9216df32be6

diff -r 000000000000 -r 20fc96400ca3 SpeedController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpeedController.cpp	Thu Jun 16 07:36:15 2016 +0000
@@ -0,0 +1,63 @@
+#include "mbed.h"
+#include "EC.h"
+
+/***ECライブラリの方を先に読んでおくこと***/
+
+
+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) {
+    Kv_p=0;Kv_p=0;Kv_i=0;diff=0;diff_old=0;integral=0;
+    out_duty=0;out=0;duty=0;
+    pwm_F_.period_us(100);
+    pwm_B_.period_us(100);
+    C=45.0;
+}
+
+void SpeedControl::Sc(double target_omega){       //スカンジウムじゃないよ
+    double now_omega=omega;
+    diff= target_omega-now_omega;
+    integral+=diff;
+    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral;
+    diff_old=diff;
+    if(out_duty>0.1)out_duty=0.1;
+    if(out_duty<-0.1)out_duty=-0.1;
+    out+=out_duty;
+    duty=0.0001*out+target_omega/C;
+    if(duty<-0.5)duty=-0.5;
+    else if(duty>0.5)duty=0.5;
+    
+    if(duty>=0){
+        pwm_F_=duty;
+        pwm_B_=0;
+    }
+    else {
+        pwm_F_=0;
+        pwm_B_=-duty;
+    }
+}
+
+
+void SpeedControl::setPIDparam(double p,double i,double d){
+    Kv_p=p;
+    Kv_d=d;
+    Kv_i=i;
+}
+
+
+void SpeedControl::setDOconstant(double c){
+    C=c;
+}
+
+void SpeedControl::stop(){
+    pwm_F_=0;
+    pwm_B_=0;
+}
+
+void SpeedControl::turnF(double duty){
+    pwm_F_=duty;
+    pwm_B_=0;
+}
+
+void SpeedControl::turnB(double duty){
+    pwm_F_=0;
+    pwm_B_=duty;
+}
\ No newline at end of file