yuto kawamura
right and left move at the same time
Diff: main.cpp
- Revision:
- 2:db2bc2ae4d20
- Parent:
- 1:34371ffd3dc0
- Child:
- 3:29999b02e940
diff -r 34371ffd3dc0 -r db2bc2ae4d20 main.cpp
--- a/main.cpp Sat Apr 27 05:34:43 2019 +0000
+++ b/main.cpp Mon Apr 29 07:00:04 2019 +0000
@@ -2,56 +2,302 @@
#include "pin.h"
+//#define DEBUG_ON
+
+#ifdef DEBUG_ON
+#define DEBUG(...) printf("" __VA_ARGS__);
+#else
+#define DEBUG(...)
+#endif
+
+#define Pi 3.14159265359 //円周率π
-////////////関数
-void setup();
-void can_receive();
-void pid(double,double);
-void out_ro(double);
-void out_ri(double);
-void reset();
+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();//ここがメインで走る記述
+};
+
-////////////定数
-int solution=1000;
-double c_degree=0.36; //solution=500
+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;
+}
-double Kp_ro=0.01;
-double Ki_ro=0;
-double Kd_ro=0;
-double Kp_ri=0.01;
+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());
+ }
+}
-double Ki_ri=0;
-double Kd_ri=0;
+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")
+ }
+
+}
////////////変数
-double target_ro=0,target_ri=0;
bool hand_mode=0;
-double distance_ro_old=0,distance_ri_old=0,pile_ro=0,pile_ri=0;
-double posi_ro=0,posi_ri=0;
-double pre_time=0;
+
+////////////関数
+void setup();
+void can_receive(float &tar_ro, float &tar_ri);
+void reset();
-
-Timer timer;
-
-
-
+//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 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.1);
+ motor_ri.setDutyLimit(0.1);
+
reset();
- while(1) {
- can_receive();
- pid(target_ro,target_ri);
- wait(0.01);
+ bus_out = 1;
+ printf("start\n\r");
+
+ float target_ro, target_ri;
+ while(1)
+ {
+ float target_ro_now, target_ri_now;
+ can_receive(target_ro_now,target_ri_now);
+ printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now);
+ if(target_ro_now != target_ro)
+ {
+ target_ro = target_ro_now;
+ target_ri = target_ri_now;
+ bus_out = 0;
+ leg_ro.setTargetPose(target_ro);
+ leg_ri.setTargetPose(target_ri);
+ robot.run();
+ }
+ motor_ro_f.write(0);
+ motor_ro_b.write(0);
+ motor_ri_f.write(0);
+ motor_ri_b.write(0);
+ bus_out = 1;
}
+
}
//////////////////////////////////////////////
@@ -67,132 +313,35 @@
switch_ri.mode(PullUp);
servo.init();
- timer.start();
}
void reset()
{
while(switch_ro.read()) {
- out_ro(0.05);
+ motor_ro.output(0.13);
}
ec_ro.reset();
- out_ro(0);
- while(switch_ri.read()) {
- out_ri(0.05);
- }
+ motor_ro.output(0.0);
+ printf("ro OK\n\r");
+ /*while(switch_ri.read()) {
+ motor_ri.output(0.13);
+ }*/
ec_ri.reset();
- out_ri(0);
-}
-
-void pid(double target_ro_,double target_ri_)
-{
- posi_ro=(ec_ro.getCount()%solution)*c_degree;
- if(posi_ro<0)posi_ro+=360;
- posi_ri=(ec_ri.getCount()%solution)*c_degree;
- if(posi_ri<0)posi_ri+=360;
-
- double now=timer.read();
- double d_time=now-pre_time;
-
- double deviation_ro=fabs(target_ro_)-posi_ro;
- if(fabs(deviation_ro)<90) { //そのまま
- } else if(deviation_ro>270) {
- deviation_ro-=360;
- } else if(deviation_ro<-270) {
- deviation_ro+=360;
- } else if(target_ro_>0) {
- if(deviation_ro<0)deviation_ro+=360;
- } else {
- if(deviation_ro>0)deviation_ro-=360;
- }
-
- double deviation_ri=fabs(target_ri_)-posi_ri;
- if(fabs(deviation_ri)<90) { //そのまま
- } else if(deviation_ri>270) {
- deviation_ri-=360;
- } else if(deviation_ri<-270) {
- deviation_ri+=360;
- } else if(target_ri_>0) {
- if(deviation_ri<0)deviation_ri+=360;
- } else {
- if(deviation_ri>0)deviation_ri-=360;
- }
-
- pile_ro+=deviation_ro;
- pile_ri+=deviation_ri;
-
- out_ro(deviation_ro * Kp_ro + (posi_ro - distance_ro_old) / d_time * Kd_ro + pile_ro * Ki_ro * d_time);
- out_ri(deviation_ri * Kp_ri + (posi_ri - distance_ri_old) / d_time * Kd_ri + pile_ri * Ki_ri * d_time);
-
- distance_ro_old=deviation_ro;
- distance_ri_old=deviation_ri;
- pre_time=now;
+ motor_ri.output(0.0);
+ printf("ri OK\n\r");
}
-void out_ro(double duty)
-{
- double dutylimit=0.1;
-
- if(duty > 0) { //入力duty比が正の場合
- //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
- if( fabs( duty ) < dutylimit ) { //制限値内
- motor_ro_f = fabs(duty);
- motor_ro_b = 0;
- } else { //制限値超
- motor_ro_f = dutylimit;
- motor_ro_b = 0;
- }
- } else {//入力duty比が負の場合
- //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max;
- if( fabs(duty) < dutylimit) { //制限値内
- motor_ro_f = 0;
- motor_ro_b = fabs(duty);
- } else { //制限値超
- motor_ro_f = 0;
- motor_ro_b = dutylimit;
- }
- }
- //pre_out_r=duty;
-}
-
-
-void out_ri(double duty)
-{
- double dutylimit=0.1;
-
- if(duty > 0) { //入力duty比が正の場合
- //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
- if( fabs( duty ) < dutylimit ) { //制限値内
- motor_ri_f = fabs(duty);
- motor_ri_b = 0;
- } else { //制限値超
- motor_ri_f = dutylimit;
- motor_ri_b = 0;
- }
- } else {//入力duty比が負の場合
- //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max;
- if( fabs(duty) < dutylimit) { //制限値内
- motor_ri_f = 0;
- motor_ri_b = fabs(duty);
- } else { //制限値超
- motor_ri_f = 0;
- motor_ri_b = dutylimit;
- }
- }
- //pre_out_r=duty;
-}
-
//////////////////////////////////////////can
-void can_receive()
+void can_receive(float &tar_ro, float &tar_ri)
{
CANMessage msg;
for(int i=0; i<5; i++) {
if(can.read(msg)) {
if(msg.id==0) {
- target_ro= msg.data[0] + ((msg.data[2]&0b1111)<<8) - 360;
- target_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360;
+ 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];
break;