Diff: main.cpp

Revision:

4:81f01f93e502

Parent:

3:7a608fbd3bcd

Child:

5:63462d696256

--- a/main.cpp	Sat Apr 27 08:13:01 2019 +0000
+++ b/main.cpp	Mon Apr 29 07:01:51 2019 +0000
@@ -2,73 +2,316 @@
 #include "pin.h"
 #include "microinfinity.h"
 
+//#define DEBUG_ON
+
+#ifdef DEBUG_ON
+#define DEBUG(...) printf("" __VA_ARGS__);
+#else
+#define DEBUG(...)
+#endif
+
+#define Pi 3.14159265359 //円周率π
+
+float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
+
+class PIDcontroller //distanceをvalueに置き換えました
+{
+    float Kp_, Ki_, Kd_, tolerance_, time_delta_;
+    float pile_, value_old_, target_;
+
+  public:
+    bool IsConvergence_;                                //収束したかどうか
+    PIDcontroller(float Kp, float Ki, float Kd);        //初期設定で係数を入力
+    void setCoefficients(float Kp, float Ki, float Kd){Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;}; //係数を変更するときに使う
+    void setTimeDelta(float delta){time_delta_ = delta;};
+    void setTarget(float target);                       //目標位置の設定
+    void setTolerance(float tolerance){tolerance_ = tolerance;}; //許容誤差の設定
+    float calc(float nowVal);           //現在位置と目標を比較してPID補正
+    bool knowConvergence(){return IsConvergence_;};             //収束したかどうかを外部に伝える
+};
+
+class Motor //PIDコントローラ、エンコーダを含むモータのクラス
+{
+    PwmOut *pin_forward_, *pin_back_;
+    Ec *ec_;                             //対応するエンコーダ
+    float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
+    int resolution_;
+  public:
+    Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
+    void setDutyLimit(float limit){duty_limit_ = limit;};
+    float getPosi();                   //ポジをエンコーダから取得
+    void calcDuty(PIDcontroller *pid); //Duty比を計算
+    void setEncoder(Ec *ec){ec_ = ec;};           //エンコーダを設定
+    void setResolution(int reso){resolution_ = reso;};
+    void output();                     //出力するだけ
+    void output(float duty);
+};
+
+class OneLeg //足の挙動を制御する
+{
+    Motor *motor_;
+    float target_pose_;
+
+  public:
+    PIDcontroller *pid_;
+    OneLeg(){};
+    void setMotor(Motor *motor){motor_ = motor;};
+    void setPIDcontroller(PIDcontroller *pid){pid_ = pid;};
+    void setTargetPose(float target_pose);
+    void actMotor();//モータ出力
+};
+
+class Robot
+{
+    float ticker_time_, air_wait_time_;
+    OneLeg *Leg1_, *Leg2_;
+    Timer timer;
+
+  public:
+    Robot(){timer.reset(); timer.start();};
+    void setLeg(OneLeg *Leg1_, OneLeg *Leg2_);
+    void setTickerTime(float ticker_time);
+    void run();//ここがメインで走る記述
+};
+
+
+
+PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
+{
+    Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
+    DEBUG("set Kp:%.3f  KI:%.3f  Kd:%.3f \n\r", Kp_, Ki_, Kd_);
+    IsConvergence_=true;
+}
+void PIDcontroller::setTarget(float target)
+{
+    if (IsConvergence_) //収束時のみ変更可能
+    {
+        target_ = target;
+        DEBUG("set Target: %.3f\n\r", target_);
+        IsConvergence_ = false;
+    }
+    else
+    {
+        DEBUG("error: setTarget permission denied!\n");
+    }
+}
+float PIDcontroller::calc(float nowVal)
+{
+    float out = 0;
+    //PID計算ここで行う
+    float deviation = target_ - nowVal; //目標との差分
+    pile_ += deviation;                 //積分用に和を取る
+    out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
+    value_old_ = nowVal; //今のデータを保存
+    //
+    if (fabs(deviation) < tolerance_) //収束した場合
+    {
+        DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
+        out = 0;
+        pile_ = 0;
+        value_old_ = 0;
+        IsConvergence_ = true;
+    }
+    return out;
+}
+
+Motor::Motor(PwmOut *forward, PwmOut *back)
+{
+    pin_forward_ = forward;
+    pin_back_ = back;
+}
+float Motor::getPosi()
+{
+    float posi = 2.0*180*((float)(ec_->getCount)()/(float)resolution_);
+    
+    //DEBUG("value :%d  :%d\n\r", (ec_->getCount)(),resolution_);
+    DEBUG("posi is %.4f\n\r",posi);
+    return posi;
+}
+void Motor::calcDuty(PIDcontroller *pid)
+{    
+    duty_ = pid->calc(getPosi());
+    DEBUG("duty is %.4f\n\r",duty_);
+}
+void Motor::output()
+{
+    //DEBUG("dutyOut %.3f\n\r",duty_);
+    //加速度が一定値を超えたら変更加える
+    if (duty_ > 0)
+    {
+        if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
+        double output_duty=min(fabs(duty_), duty_limit_);
+        pin_forward_->write(output_duty);
+        pin_back_->write(0);
+        DEBUG("forward %.3f\n\r",pin_forward_->read());
+    }
+    else
+    {
+        if (pre_duty_ - duty_ > accel_max)
+            duty_ = pre_duty_ - accel_max;
+        double output_duty=min(fabs(duty_), duty_limit_);
+        pin_forward_->write(0);
+        pin_back_->write(output_duty);
+        DEBUG("back %.3f\n\r",pin_back_->read());
+    }
+    pre_duty_ = duty_;
+}
+void Motor::output(float duty)
+{
+    duty_ = duty;
+    //DEBUG("dutyOut %.3f\n\r",duty_);
+    //加速度が一定値を超えたら変更加える
+    if (duty_ > 0)
+    {
+        //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
+        double output_duty=min(fabs(duty_), duty_limit_);
+        pin_forward_->write(output_duty);
+        pin_back_->write(0);
+        DEBUG("forward %.3f\n\r",pin_forward_->read());
+    }
+    else
+    {
+        //if (pre_duty_ - duty_ > accel_max)
+        //    duty_ = pre_duty_ - accel_max;
+        double output_duty=min(fabs(duty_), duty_limit_);
+        pin_forward_->write(0);
+        pin_back_->write(output_duty);
+        DEBUG("back %.3f\n\r",pin_back_->read());
+    }
+    pre_duty_ = duty_;
+}
+
+void OneLeg::setTargetPose(float target_pose)
+{
+    target_pose_ = target_pose;
+    //PIDにtargetを送る
+    pid_->setTarget(target_pose_);
+}
+void OneLeg::actMotor()
+{
+    motor_->calcDuty(pid_);
+    motor_->output();
+}
+
+
+
+void Robot::setLeg(OneLeg *Leg1, OneLeg *Leg2)
+{
+    Leg1_ = Leg1;
+    Leg2_ = Leg2;
+}
+void Robot::setTickerTime(float ticker_time)
+{
+    ticker_time_ = ticker_time;
+    Leg1_->pid_->setTimeDelta(ticker_time_);
+    Leg2_->pid_->setTimeDelta(ticker_time_);
+}
+void Robot::run()
+{
+    while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) //片方が収束していない時*/
+    {
+        //ticker_time毎にモータに出力する
+        float time_s = timer.read();
+        Leg1_->actMotor();
+        Leg2_->actMotor();
+        float rest_time_s = ticker_time_ - (timer.read() - time_s);
+        //ticker_timeまで待機
+        if (rest_time_s > 0)
+        {
+            wait(rest_time_s);
+            DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
+        }
+            
+        else //時間が足りない場合警告
+            printf("error: restTime not enough\n\r");
+        DEBUG("loop end\n\r")
+    }
+
+}
+
 
 
 ////////////関数
+void reset();
 void setup();
-void can_send();
-void pid(double,double);
-void out_lo(double);
-void out_li(double);
-void reset();
+void can_send(float target_ro, float target_ri);
 
 ////////////定数
-int resolution_lo=1000;
-int resolution_li=600;
-double c_degree_lo=0.36;   //solution=500
-double c_degree_li=360.0/600.0;
-
-double Kp_lo=0.01;
-double Ki_lo=0;
-double Kd_lo=0;
-double Kp_li=0.01;
-
-double Ki_li=0;
-double Kd_li=0;
-
-
 
 ////////////変数
-double target_ro=0,target_ri=0;
-double target_lo=0,target_li=0;
 bool hand_mode=0;
-double distance_lo_old=0,distance_li_old=0,pile_lo=0,pile_li=0;
-double posi_lo=0,posi_li=0;
-double pre_time=0;
 
-Timer timer;
-
-
-
+//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
+//しかし変更を多々行うためポインタ渡しにしてある
+//文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
+PIDcontroller pid_lo(0.01, 0.000, 0.000);
+PIDcontroller pid_li(0.01, 0.000, 0.000);    //Kp.Ki,Kd
+Motor motor_lo(&motor_lo_f, &motor_lo_b),
+     motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
+OneLeg leg_lo, leg_li;
+Robot robot;
 
 /////////////////////////////////////////////
 int main()
 {
-
     setup();
+    
+    pid_lo.setTolerance(10);
+    pid_li.setTolerance(10);
+    
+    motor_lo.setEncoder(&ec_lo);
+    motor_lo.setResolution(1000);
+    motor_li.setEncoder(&ec_li);
+    motor_li.setResolution(600);
+    
+    leg_lo.setMotor(&motor_lo);
+    leg_lo.setPIDcontroller(&pid_lo);
+    leg_li.setMotor(&motor_li);
+    leg_li.setPIDcontroller(&pid_li);
+    
+    robot.setLeg(&leg_lo, &leg_li);
+    robot.setTickerTime(0.01); //モータ出力間隔 0.01
+    
+    motor_lo.setDutyLimit(0.1);
+    motor_li.setDutyLimit(0.1);
+    
+    /*
+    char str[255] = {};
+    printf("setup complete Input any key\n\r");
+    scanf("%s", str);
+    printf("start!");
+    */
+    
     reset();
-    while(1){
-    target_lo = 180;
-    target_li = 360;
-    target_ri = 360;
-    target_ro = 180;
-    for(int i=0;i<100;++i){
-       can_send();
-       pid(target_lo,target_li);
-       //printf("%f,%f\r\n",posi_li,posi_lo);
-       wait(0.01);
+    printf("bus standby\n\r");
+    while(1)
+    {
+        if(bus_in.read() == 1) break;     
     }
-    target_lo = 360;
-    target_li = 180;
-    target_ri = 180;
-    target_ro = 360;
-    for(int i=0;i<100;++i){
-       can_send();
-       pid(target_lo,target_li);
-       //printf("%f,%f\r\n",posi_li,posi_lo);
-       wait(0.01);
+    printf("bus is %d\n\r", bus_in.read());
+    
+    //Sample
+    for(int i = 0; i< 20; ++i)
+    {
+        can_send(360+i*180,180+i*180);
+        leg_lo.setTargetPose(360+i*180);
+        leg_li.setTargetPose(180+i*180);
+        robot.run();
+        motor_lo_f.write(0);
+        motor_lo_b.write(0);
+        motor_li_f.write(0);
+        motor_li_b.write(0);
+        while(1)
+        {
+            if(bus_in.read() == 1) break;     
+        }
     }
-    }
+    
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    
 }
 
 
@@ -92,12 +335,11 @@
     wait(0.05);
     theta0=degree0;
     check_gyro();
-    timer.start();
 }
 
 
 //////////////////////////////////////can
-void can_send()
+void can_send(float target_ro, float target_ri)
 {
     char data[4]= {0};
     int target_ro_send=target_ro+360;
@@ -110,118 +352,17 @@
     if(can1.write(CANMessage(0,data,4)))led4=1;
     else led4=0;
 }
-
 void reset()
 {
     while(switch_lo.read()) {
-        out_lo(0.05);
+        motor_lo.output(0.07);
     }
      ec_lo.reset();
-     out_lo(0);
+     motor_lo.output(0.0);
     while(switch_li.read()) {
-        out_li(0.05);
+        motor_li.output(0.07);
     }
-    
      ec_li.reset();
-     out_li(0);
-}
-
-void out_lo(double duty)
-{
-  
-    double dutylimit=0.1;
-
-    if(duty > 0) { //入力duty比が正の場合
-        //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
-        if( fabs( duty ) < dutylimit ) { //制限値内
-            motor_lo_f = fabs(duty);
-            motor_lo_b = 0;
-        } else { //制限値超
-            motor_lo_f = dutylimit;
-            motor_lo_b = 0;
-        }
-    } else {//入力duty比が負の場合
-        //if(pre_out_r-duty >accel_max  && pre_out_l*duty>0)duty=pre_out_r-accel_max;
-        if( fabs(duty) < dutylimit) { //制限値内
-            motor_lo_f = 0;
-            motor_lo_b = fabs(duty);
-        } else { //制限値超
-            motor_lo_f = 0;
-            motor_lo_b = dutylimit;
-        }
-    }
-    //pre_out_r=duty;
+     motor_li.output(0.0);
 }
 
-
-void out_li(double duty)
-{
-   
-    double dutylimit=0.1;
-
-    if(duty > 0) { //入力duty比が正の場合
-        //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
-        if( fabs( duty ) < dutylimit ) { //制限値内
-            motor_li_f = fabs(duty);
-            motor_li_b = 0;
-        } else { //制限値超
-            motor_li_f = dutylimit;
-            motor_li_b = 0;
-        }
-    } else {//入力duty比が負の場合
-        //if(pre_out_r-duty >accel_max  && pre_out_l*duty>0)duty=pre_out_r-accel_max;
-        if( fabs(duty) < dutylimit) { //制限値内
-            motor_li_f = 0;
-            motor_li_b = fabs(duty);
-        } else { //制限値超
-            motor_li_f = 0;
-            motor_li_b = dutylimit;
-        }
-    }
-    //pre_out_r=duty;
-}
-
-void pid(double target_lo_,double target_li_)
-{
-    posi_lo=(ec_lo.getCount()%resolution_lo)*c_degree_lo;
-    if(posi_lo<0)posi_lo+=360;
-    posi_li=(ec_li.getCount()%resolution_li)*c_degree_li;
-    if(posi_li<0)posi_li+=360;
-
-    double now=timer.read();
-    double d_time=now-pre_time;
-
-    double deviation_lo=fabs(target_lo_)-posi_lo;
-    if(fabs(deviation_lo)<90) { //そのまま
-    } else if(deviation_lo>270) {
-        deviation_lo-=360;
-    } else if(deviation_lo<-270) {
-        deviation_lo+=360;
-    } else if(target_lo_>0) {
-        if(deviation_lo<0)deviation_lo+=360;
-    } else {
-        if(deviation_lo>0)deviation_lo-=360;
-    }
-
-    double deviation_li=fabs(target_li_)-posi_li;
-    if(fabs(deviation_li)<90) { //そのまま
-    } else if(deviation_li>270) {
-        deviation_li-=360;
-    } else if(deviation_li<-270) {
-        deviation_li+=360;
-    } else if(target_li_>0) {
-        if(deviation_li<0)deviation_li+=360;
-    } else {
-        if(deviation_li>0)deviation_li-=360;
-    }
-
-    pile_lo+=deviation_lo;
-    pile_li+=deviation_li;
-
-    out_lo(deviation_lo * Kp_lo + (posi_lo - distance_lo_old) / d_time * Kd_lo + pile_lo *  Ki_lo * d_time);
-    out_li(deviation_li * Kp_li + (posi_li - distance_li_old) / d_time * Kd_li + pile_li *  Ki_li * d_time);
-
-    distance_lo_old=deviation_lo;
-    distance_li_old=deviation_li;
-    pre_time=now;
-}
\ No newline at end of file