![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
right and left move at the same time
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-05-02
- Revision:
- 7:c5c60192eb02
- Parent:
- 6:2bc2950f32d8
- Child:
- 8:43c31d2afb9e
File content as of revision 7:c5c60192eb02:
#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 { BACK, STOP, FORWARD, }; enum EVENT { NORMAL, GEREGE, 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") } } ////////////変数 bool hand_mode=NORMAL; ////////////関数 void setup(); void forward(); void stop(); void back(); void can_receive(int &mode, float &duty); void reset(); //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う //しかし変更を多々行うためポインタ渡しにしてある //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う PIDcontroller pid_ro(0.01, 0.000, 0.000); PIDcontroller pid_ri(0.01, 0.000, 0.000); //Kp.Ki,Kd Motor motor_ro(&motor_ro_f, &motor_ro_b), motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入 OneLeg leg_ro, leg_ri; Robot robot; int step_num_r = 0; ////////////////////////////////////////////// int main() { printf("standby ok\n\r"); setup(); pid_ro.setTolerance(10); pid_ri.setTolerance(10); motor_ro.setEncoder(&ec_ro); motor_ro.setResolution(1000); motor_ri.setEncoder(&ec_ri); motor_ri.setResolution(1000); leg_ro.setMotor(&motor_ro); leg_ro.setPIDcontroller(&pid_ro); leg_ri.setMotor(&motor_ri); leg_ri.setPIDcontroller(&pid_ri); robot.setLeg(&leg_ro, &leg_ri); robot.setTickerTime(0.01); //モータ出力間隔 0.01 motor_ro.setDutyLimit(0.5); motor_ri.setDutyLimit(0.5); reset(); bus_out = 1; printf("start\n\r"); int mode = -1; air_hand = 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) { air_hand = 0; } else if(hand_mode == GOAL) { for(int i=0;i<5;++i) servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500)); } bus_out = 0; //printf("target is %d\n\r",target_ro); if(mode == FORWARD) forward(); else if(mode == STOP) stop(); else if(mode == BACK) back(); bus_out = 1; } } void forward() { leg_ro.setTargetPose(360+step_num_r*180); leg_ri.setTargetPose(180+step_num_r*180); robot.run(); motor_ro_f.write(0); motor_ro_b.write(0); motor_ri_f.write(0); motor_ri_b.write(0); step_num_r++; } void stop() { leg_ro.setTargetPose(360+(step_num_r-1)*180); leg_ri.setTargetPose(180+(step_num_r-1)*180); robot.run(); motor_ro_f.write(0); motor_ro_b.write(0); motor_ri_f.write(0); motor_ri_b.write(0); } void back() { leg_ro.setTargetPose(360+(step_num_r-2)*180); leg_ri.setTargetPose(180+(step_num_r-2)*180); robot.run(); motor_ro_f.write(0); motor_ro_b.write(0); motor_ri_f.write(0); motor_ri_b.write(0); step_num_r--; } ////////////////////////////////////////////// void setup() { can.frequency(1000000); motor_ro_f.period_us(100); motor_ro_b.period_us(100); motor_ri_f.period_us(100); motor_ri_b.period_us(100); switch_ro.mode(PullUp); switch_ri.mode(PullUp); servo.init(); wait(0.01); for(int i=0;i<5;++i) servo.set_degree(0,0); //(7200 - 3500) * 270.0/(11500 - 3500) } void reset() { while(switch_ro.read()) { motor_ro.output(0.3); } ec_ro.reset(); motor_ro.output(0.0); printf("ro OK\n\r"); while(switch_ri.read()) { motor_ri.output(0.3); } ec_ri.reset(); motor_ri.output(0.0); printf("ri OK\n\r"); } //////////////////////////////////////////can void can_receive(int &mode, float &duty) { CANMessage msg; while(1) { //printf("roop\r\n"); if(can.read(msg)) { if(msg.id==0) { mode= msg.data[0]*0.1; duty = (msg.data[0]%10)*0.1; hand_mode= msg.data[1]; break; } led2=1; } else led2=0; } }