![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
l
Diff: main.cpp
- Revision:
- 6:75cfa1a66382
- Parent:
- 5:1fa5aa097af5
--- a/main.cpp Sat Apr 20 11:41:38 2019 +0000 +++ b/main.cpp Sat Apr 27 11:22:00 2019 +0000 @@ -5,7 +5,7 @@ #include "pin.h" #include "microinfinity.h" -#define DEBUG_ON +//#define DEBUG_ON #ifdef DEBUG_ON #define DEBUG(...) printf("" __VA_ARGS__); @@ -86,7 +86,7 @@ Timer timer; public: - Robot(){}; + Robot(){timer.reset(); timer.start();}; void setLeg(OneLeg *rightLeg, OneLeg *leftLeg); void setTickerTime(float ticker_time); void setAirWaitTime(float air_wait_time); @@ -185,16 +185,18 @@ if (duty_ > 0) { if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max; - *pin_forward_ = min(fabs(duty_), duty_limit_); - *pin_back_ = 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 { if (pre_duty_ - duty_ > accel_max) duty_ = pre_duty_ - accel_max; - *pin_forward_ = 0; - *pin_back_ = min(fabs(duty_), duty_limit_); + 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_; @@ -270,7 +272,7 @@ } void Robot::run() { - while (!rightLeg_->pid_->IsConvergence_ || !leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/ + while (/*!rightLeg_->pid_->IsConvergence_ || */!leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/ { //ticker_time毎にモータに出力する float time_s = timer.read(); @@ -279,9 +281,13 @@ 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 //時間が足りない場合警告 - DEBUG("error: restTime not enough\n\r"); + printf("error: restTime not enough\n\r"); DEBUG("loop end\n\r") } @@ -301,8 +307,8 @@ //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う //しかし変更を多々行うためポインタ渡しにしてある //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う -PIDcontroller pid_r(0.0015, 0.000, 0.000); -PIDcontroller pid_l(0.0015, 0.000, 0.000); //Kp.Ki,Kd +PIDcontroller pid_r(0.002, 0.000, 0.000); +PIDcontroller pid_l(0.002, 0.000, 0.000); //Kp.Ki,Kd Motor motorLeft(&motor_l_f, &motor_l_b), motorRight(&motor_r_f, &motor_r_b); //forward,backのピンを代入 OneLeg leftLeg, rightLeg; @@ -313,15 +319,14 @@ { DEBUG("start\n\r"); setup(); + //PIDコントローラ生成 左右2つで、係数の変更はメンバ関数を用いて行う //許容誤差設定 pid_r.setTolerance(20.0); pid_l.setTolerance(20.0); //モータ生成 PID、ECの代入を行うこと motorLeft.setEncoder(&ec_l); - motorLeft.setDutyLimit(0.2); motorRight.setEncoder(&ec_r); - motorRight.setDutyLimit(0.2); AirCylinder air_leg[4] = {//名前は分けたほうがいいきがする? AirCylinder(&air[0]), @@ -348,26 +353,32 @@ robot.setLeg(&rightLeg, &leftLeg); robot.setTickerTime(0.01); //モータ出力間隔 0.01 robot.setAirWaitTime(0.2); //シリンダを待つ時間 + + motorLeft.setDutyLimit(0.2); + motorRight.setDutyLimit(0.2); //初期位置は0これは内側が一番内側の時 - - //example これで1サイクル - leftLeg.setTargetPose(200.0); - rightLeg.setTargetPose(200.0); + char str[255] = {}; + printf("setup complete Input any key\n\r"); + scanf("%s", str); + printf("start!"); + leftLeg.setTargetPose(2000.0); + rightLeg.setTargetPose(2000.0); robot.run(); leftLeg.setTargetPose(80.0); rightLeg.setTargetPose(80.0); robot.run(); + } // DEBUG("program end\n\n\r"); } void setup() { - motor_r_f.period_us(100); - motor_r_b.period_us(100); - motor_l_f.period_us(100); - motor_l_b.period_us(100); + motor_r_f.period_us(50); + motor_r_b.period_us(50); + motor_l_f.period_us(50); + motor_l_b.period_us(50); for (int i = 0; i < 4; i++) {