yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
Diff: main.cpp
- Revision:
- 7:c5c60192eb02
- Parent:
- 6:2bc2950f32d8
- Child:
- 8:43c31d2afb9e
diff -r 2bc2950f32d8 -r c5c60192eb02 main.cpp --- a/main.cpp Wed May 01 08:12:51 2019 +0000 +++ b/main.cpp Thu May 02 04:53:06 2019 +0000 @@ -12,6 +12,18 @@ #define Pi 3.14159265359 //円周率π +enum WALKMODE +{ + BACK, + STOP, + FORWARD, +}; +enum EVENT +{ + NORMAL, + GEREGE, + GOAL, +}; float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。 @@ -20,15 +32,27 @@ float Kp_, Ki_, Kd_, tolerance_, time_delta_; float pile_, value_old_, target_; - public: +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 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;}; //許容誤差の設定 + void setTolerance(float tolerance) + { + tolerance_ = tolerance; + }; //許容誤差の設定 float calc(float nowVal); //現在位置と目標を比較してPID補正 - bool knowConvergence(){return IsConvergence_;}; //収束したかどうかを外部に伝える + bool knowConvergence() + { + return IsConvergence_; + }; //収束したかどうかを外部に伝える }; class Motor //PIDコントローラ、エンコーダを含むモータのクラス @@ -37,13 +61,22 @@ Ec *ec_; //対応するエンコーダ float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置 int resolution_; - public: +public: Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し - void setDutyLimit(float limit){duty_limit_ = limit;}; + 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 setEncoder(Ec *ec) + { + ec_ = ec; + }; //エンコーダを設定 + void setResolution(int reso) + { + resolution_ = reso; + }; void output(); //出力するだけ void output(float duty); }; @@ -53,11 +86,17 @@ Motor *motor_; float target_pose_; - public: +public: PIDcontroller *pid_; - OneLeg(){}; - void setMotor(Motor *motor){motor_ = motor;}; - void setPIDcontroller(PIDcontroller *pid){pid_ = pid;}; + OneLeg() {}; + void setMotor(Motor *motor) + { + motor_ = motor; + }; + void setPIDcontroller(PIDcontroller *pid) + { + pid_ = pid; + }; void setTargetPose(float target_pose); void actMotor();//モータ出力 }; @@ -68,8 +107,12 @@ OneLeg *Leg1_, *Leg2_; Timer timer; - public: - Robot(){timer.reset(); timer.start();}; +public: + Robot() + { + timer.reset(); + timer.start(); + }; void setLeg(OneLeg *Leg1_, OneLeg *Leg2_); void setTickerTime(float ticker_time); void run();//ここがメインで走る記述 @@ -85,14 +128,11 @@ } void PIDcontroller::setTarget(float target) { - if (IsConvergence_) //収束時のみ変更可能 - { + if (IsConvergence_) { //収束時のみ変更可能 target_ = target; DEBUG("set Target: %.3f\n\r", target_); IsConvergence_ = false; - } - else - { + } else { DEBUG("error: setTarget permission denied!\n"); } } @@ -105,8 +145,7 @@ out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_; value_old_ = nowVal; //今のデータを保存 // - if (fabs(deviation) < tolerance_) //収束した場合 - { + if (fabs(deviation) < tolerance_) { //収束した場合 DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r"); out = 0; pile_ = 0; @@ -124,13 +163,13 @@ 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_); } @@ -138,16 +177,13 @@ { //DEBUG("dutyOut %.3f\n\r",duty_); //加速度が一定値を超えたら変更加える - if (duty_ > 0) - { + 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 - { + } else { //if (pre_duty_ - duty_ > accel_max) // duty_ = pre_duty_ - accel_max; double output_duty=min(fabs(duty_), duty_limit_); @@ -162,15 +198,12 @@ duty_ = duty; //DEBUG("dutyOut %.3f\n\r",duty_); //加速度が一定値を超えたら変更加える - if (duty_ > 0) - { + 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 - { + } else { double output_duty=min(fabs(duty_), duty_limit_); pin_forward_->write(0); pin_back_->write(output_duty); @@ -205,20 +238,18 @@ } void Robot::run() { - while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) //片方が収束していない時*/ - { + 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) - { + 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") @@ -230,108 +261,120 @@ ////////////変数 -bool hand_mode=0; +bool hand_mode=NORMAL; ////////////関数 void setup(); -void straight(int &step_num_l, int &step_num_r); -void turnLeft(int &step_num_l, int &step_num_r); -void turnRight(int &step_num_l, int &step_num_r); -void can_receive(float &tar_ro, float &tar_ri); +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のピンを代入 + 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; - int target_ro, target_ri; while(1) { - float target_ro_now = target_ro; - float target_ri_now = target_ri; - can_receive(target_ro_now,target_ri_now); - //printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now); - if((int)target_ro_now != target_ro) + 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) { - target_ro = (int)target_ro_now; - target_ri = (int)target_ri_now; - bus_out = 0; - //printf("target is %d\n\r",target_ro); - straight(target_ro, target_ri); + 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 straight(int &step_num_l, int &step_num_r) +void forward() { - leg_ro.setTargetPose(360+step_num_l*180); - leg_ri.setTargetPose(180+step_num_l*180); + 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 turnLeft(int &step_num_l, int &step_num_r) + +void back() { - leg_ro.setTargetPose(360+(step_num_l-2)*180); - leg_ri.setTargetPose(180+(step_num_l-2)*180); + 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); -} -void turnRight(int &step_num_l, int &step_num_r) -{ - leg_ro.setTargetPose(360+step_num_l*180); - leg_ri.setTargetPose(180+step_num_l*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--; } @@ -348,6 +391,10 @@ 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) } @@ -357,35 +404,35 @@ while(switch_ro.read()) { motor_ro.output(0.3); } - ec_ro.reset(); - motor_ro.output(0.0); - printf("ro OK\n\r"); + 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"); + + ec_ri.reset(); + motor_ri.output(0.0); + printf("ri OK\n\r"); } //////////////////////////////////////////can -void can_receive(float &tar_ro, float &tar_ri) +void can_receive(int &mode, float &duty) { CANMessage msg; - for(int i=0; i<5; i++) { + while(1) { + //printf("roop\r\n"); if(can.read(msg)) { if(msg.id==0) { - tar_ro= msg.data[0] + ((msg.data[2]&0b1111)<<8) - 360; - tar_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360; - hand_mode= msg.data[3]; - + mode= msg.data[0]*0.1; + duty = (msg.data[0]%10)*0.1; + hand_mode= msg.data[1]; break; } led2=1; } else led2=0; } -} +} \ No newline at end of file