Diff: main.cpp

Revision:

9:ac95473a5d86

Parent:

8:43c31d2afb9e

Child:

10:2973cea54efd

diff -r 43c31d2afb9e -r ac95473a5d86 main.cpp
--- a/main.cpp	Thu May 02 09:08:26 2019 +0000
+++ b/main.cpp	Wed May 08 02:56:21 2019 +0000
@@ -1,269 +1,21 @@
 #include "mbed.h"
 #include "pin.h"
-
-
-//#define DEBUG_ON
-
-#ifdef DEBUG_ON
-#define DEBUG(...) printf("" __VA_ARGS__);
-#else
-#define DEBUG(...)
-#endif
-
-#define Pi 3.14159265359 //円周率π
-
-enum WALKMODE
-{
+#include "debug.h"
+#include "robot.h"
+enum WALKMODE {
     BACK,
     STOP,
     FORWARD,
 };
-enum EVENT
-{
+enum EVENT {
     NORMAL,
     GEREGE,
-    GOAL,    
+    GOAL,
 };
 
 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) {
-        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 {
-        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());
-    }
-}
-
-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")
-    }
-
-}
-
-
-
-
-////////////変数
 int hand_mode=NORMAL;
 
-////////////関数
 void setup();
 void forward();
 void stop();
@@ -283,13 +35,10 @@
 Robot robot;
 
 int step_num_r = 0;
-//////////////////////////////////////////////
 int main()
 {
     printf("standby ok\n\r");
     setup();
-
-
     pid_ro.setTolerance(10);
     pid_ri.setTolerance(10);
 
@@ -313,24 +62,19 @@
     bus_out = 1;
     printf("start\n\r");
     int mode = -1;
-    
     air_hand = 1;
-    
-    while(1) 
-    {
+
+    while(1) {
         float duty = 0.0;
         can_receive(mode,duty);
         printf("mode:%d duty:%.3f\n\r", mode, duty);
         motor_ro.setDutyLimit(duty);
         motor_ri.setDutyLimit(duty);
-        if(hand_mode == GEREGE)
-        {
+        if(hand_mode == GEREGE) {
             air_hand = 0;
-        }
-        else if(hand_mode == GOAL)
-        {
-            for(int i=0;i<10;++i)
-                servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500));    
+        } else if(hand_mode == GOAL) {
+            for(int i=0; i<10; ++i)
+                servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500));
         }
         bus_out = 0;
         //printf("target is %d\n\r",target_ro);
@@ -353,7 +97,6 @@
     motor_ri_b.write(0);
     step_num_r++;
 }
-
 void stop()
 {
     leg_ro.setTargetPose(360+(step_num_r-1)*180);
@@ -376,8 +119,6 @@
     motor_ri_b.write(0);
     step_num_r--;
 }
-
-
 //////////////////////////////////////////////
 void setup()
 {
@@ -386,19 +127,16 @@
     motor_ro_b.period_us(100);
     motor_ri_f.period_us(100);
     motor_ri_b.period_us(100);
-
+    air_hand = 1;//ゲレゲを離す手の形にしておく
     switch_ro.mode(PullUp);
     switch_ri.mode(PullUp);
-
     servo.init();
     wait(0.01);
-    for(int i=0;i<5;++i)
+    for(int i=0; i<5; ++i)
         servo.set_degree(0,0);
     //(7200 - 3500) * 270.0/(11500 - 3500)
 }
 
-
-
 void reset()
 {
     while(switch_ro.read()) {
@@ -416,10 +154,6 @@
     printf("ri OK\n\r");
 }
 
-
-
-
-//////////////////////////////////////////can
 void can_receive(int &mode, float &duty)
 {
     CANMessage msg;