yuto kawamura
/
MR2_n_1_class
l
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-04-20
- Revision:
- 5:1fa5aa097af5
- Parent:
- 4:3f59e4f5ca30
- Child:
- 6:75cfa1a66382
File content as of revision 5:1fa5aa097af5:
#include "mbed.h" #include "EC.h" #include "KondoServo.h" #include "hcsr04.h" #include "pin.h" #include "microinfinity.h" #define DEBUG_ON #ifdef DEBUG_ON #define DEBUG(...) printf("" __VA_ARGS__); #else #define DEBUG(...) #endif int ecgear = 25; //ここはいちいち設定したくないからグローバル float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。 #define Pi 3.14159265359 //円周率π 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); //係数を変更するときに使う void setTimeDelta(float delta); void setTarget(float target); //目標位置の設定 void setTolerance(float tolerance); //許容誤差の設定 float calc(float nowVal); //現在位置と目標を比較してPID補正 bool knowConvergence(); //収束したかどうかを外部に伝える }; class Motor //PIDコントローラ、エンコーダを含むモータのクラス { PwmOut *pin_forward_, *pin_back_; Ec *ec_; //対応するエンコーダ float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置 public: Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し void setDutyLimit(float limit); float getPosi(); //ポジをエンコーダから取得 void calcDuty(PIDcontroller *pid); //Duty比を計算 void setEncoder(Ec *ec); //エンコーダを設定 void output(); //出力するだけ }; class AirCylinder { DigitalOut *air_; bool IsOpenState_; public: AirCylinder(DigitalOut *air); //ピンをポインタ渡し void open(); //足上げ void close(); //足下げ void changeState(); //上がってるとき下げる その逆も }; class OneLeg //足の挙動を制御する { Motor *motor_; AirCylinder *air1_, *air2_; float target_pose_; public: PIDcontroller *pid_; OneLeg(){}; void setMotor(Motor *motor); void setPIDcontroller(PIDcontroller *pid); void setAir(AirCylinder *air1, AirCylinder *air2); void setTargetPose(float target_pose); void actMotor();//モータ出力 void actCylinder();//シリンダ出力 }; class Robot { float ticker_time_, air_wait_time_; OneLeg *rightLeg_, *leftLeg_; Timer timer; public: Robot(){}; void setLeg(OneLeg *rightLeg, OneLeg *leftLeg); void setTickerTime(float ticker_time); void setAirWaitTime(float air_wait_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::setCoefficients(float Kp, float Ki, float Kd) { Kp_ = Kp, Ki_ = Ki, Kd_ = Kd; } void PIDcontroller::setTimeDelta(float delta) { time_delta_ = delta; DEBUG("set TimeDelta: %.3f\n\r", time_delta_); } 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"); } } void PIDcontroller::setTolerance(float tolerance) { tolerance_ = tolerance; DEBUG("set Tolerance: %.3f\n\r", tolerance_); } 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; } bool PIDcontroller::knowConvergence() { return IsConvergence_; } Motor::Motor(PwmOut *forward, PwmOut *back) { pin_forward_ = forward; pin_back_ = back; } float Motor::getPosi() { float posi = 1.0 * ecgear * Pi * (ec_->getCount)() /resolution; DEBUG("posi is %.4f\n\r",posi); return posi; } void Motor::setDutyLimit(float limit) { duty_limit_ = limit; DEBUG("set DutyLimit: %.3f\n\r", duty_limit_); } void Motor::calcDuty(PIDcontroller *pid) { duty_ = pid->calc(getPosi()); DEBUG("duty is %.4f\n\r",duty_); } void Motor::setEncoder(Ec *ec) { ec_ = ec; } void Motor::output() { //DEBUG("dutyOut %.3f\n\r",duty_); //加速度が一定値を超えたら変更加える if (duty_ > 0) { if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max; *pin_forward_ = min(fabs(duty_), duty_limit_); *pin_back_ = 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_); DEBUG("back %.3f\n\r",pin_back_->read()); } pre_duty_ = duty_; } AirCylinder::AirCylinder(DigitalOut *air) { air_ = air; } void AirCylinder::open() { *air_ = 1; IsOpenState_ = true; } void AirCylinder::close() { *air_ = 0; IsOpenState_ = false; } void AirCylinder::changeState() { if (IsOpenState_) close(); else open(); } void OneLeg::setMotor(Motor *motor) { motor_ = motor; } void OneLeg::setPIDcontroller(PIDcontroller *pid) { pid_ = pid; } void OneLeg::setAir(AirCylinder *air1, AirCylinder *air2) { air1_ = air1, air2_ = air2; } 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 OneLeg::actCylinder() { //上がっている場合は下げ、下がっている時は上げる air1_->changeState(); air2_->changeState(); } void Robot::setLeg(OneLeg *rightLeg, OneLeg *leftLeg) { rightLeg_ = rightLeg; leftLeg_ = leftLeg; } void Robot::setTickerTime(float ticker_time) { ticker_time_ = ticker_time; rightLeg_->pid_->setTimeDelta(ticker_time_); leftLeg_->pid_->setTimeDelta(ticker_time_); } void Robot::setAirWaitTime(float air_wait_time) { air_wait_time_ = air_wait_time; DEBUG("set AirWaitTime: %.3f\n\r", air_wait_time_); } void Robot::run() { while (!rightLeg_->pid_->IsConvergence_ || !leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/ { //ticker_time毎にモータに出力する float time_s = timer.read(); rightLeg_->actMotor(); leftLeg_->actMotor(); float rest_time_s = ticker_time_ - (timer.read() - time_s); //ticker_timeまで待機 if (rest_time_s > 0) wait(rest_time_s); else //時間が足りない場合警告 DEBUG("error: restTime not enough\n\r"); DEBUG("loop end\n\r") } rightLeg_->actCylinder(); leftLeg_->actCylinder(); wait(air_wait_time_); } void setup(); void wait_MR1(); //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 Motor motorLeft(&motor_l_f, &motor_l_b), motorRight(&motor_r_f, &motor_r_b); //forward,backのピンを代入 OneLeg leftLeg, rightLeg; Robot robot; int main() { 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]), AirCylinder(&air[1]), AirCylinder(&air[2]), AirCylinder(&air[3])}; AirCylinder air_nonleg[2] = { AirCylinder(&air[4]), AirCylinder(&air[5])}; //シリンダ初期設定 air_leg[0].open(); air_leg[1].close();//内側の二つが開く air_leg[2].close(); air_leg[3].open(); leftLeg.setMotor(&motorLeft); leftLeg.setPIDcontroller(&pid_l); leftLeg.setAir(&air_leg[0], &air_leg[1]); rightLeg.setMotor(&motorRight); rightLeg.setPIDcontroller(&pid_r); rightLeg.setAir(&air_leg[2], &air_leg[3]); robot.setLeg(&rightLeg, &leftLeg); robot.setTickerTime(0.01); //モータ出力間隔 0.01 robot.setAirWaitTime(0.2); //シリンダを待つ時間 //初期位置は0これは内側が一番内側の時 //example これで1サイクル leftLeg.setTargetPose(200.0); rightLeg.setTargetPose(200.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); for (int i = 0; i < 4; i++) { air[i] = 1; } air[4] = 0; air[5] = 0; servo.init(); wait_us(1000); servo.set_degree(0, 105); //初期角度105 device.baud(115200); device.format(8, Serial::None, 1); device.attach(dev_rx, Serial::RxIrq); wait(0.05); theta0 = degree0; } void wait_MR1() { while (switch_x == 1) { } //while(switch_hand==0); //air_hand=1; wait(1); }