yuto kawamura
right and left move at the same time
Diff: main.cpp
- Revision:
- 7:c5c60192eb02
- Parent:
- 6:2bc2950f32d8
- Child:
- 8:43c31d2afb9e
--- 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