yuto kawamura
l
Diff: main.cpp
- Revision:
- 5:1fa5aa097af5
- Parent:
- 4:3f59e4f5ca30
- Child:
- 6:75cfa1a66382
diff -r 3f59e4f5ca30 -r 1fa5aa097af5 main.cpp
--- a/main.cpp Mon Apr 01 02:20:07 2019 +0000
+++ b/main.cpp Sat Apr 20 11:41:38 2019 +0000
@@ -5,55 +5,290 @@
#include "pin.h"
#include "microinfinity.h"
+#define DEBUG_ON
-Timer timer;
+#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(); //出力するだけ
+};
-//パラメタ
-double dutylimit_nomal = 0.2; // duty出力の上限値(通常時)
-double dutylimit_turn = 0.4; // duty出力の上限値（回転時）
-int ecgear = 25; //エンコーダのギアの歯数
-/*double Kp_r = 0.342000; //P値
-double Ki_r = 0.013062; //I値
-double Kd_r = 0.003266; //D値
-double Kp_l = 0.342000; //P値
-double Ki_l= 0.013062; //I値
-double Kd_l = 0.003266; //D値*/
-double Kp_r_n = 0.0015; //P値 通常時右
-double Ki_r_n = 0.001; //I値
-double Kd_r_n = 0.0001; //D値
-double Kp_l_n = 0.0015; //P値 回転時右
-double Ki_l_n = 0.001; //I値
-double Kd_l_n = 0.0001; //D値
-double Kp_r_t = 0.004; //P値
-double Ki_r_t = 0.001; //I値
-double Kd_r_t = 0.0001; //D値
-double Kp_l_t = 0.004; //P値
-double Ki_l_t = 0.001; //I値
-double Kd_l_t = 0.0001; //D値
-double leg_wait_time = 0.2; //脚の伸縮にかかる時間
-double leg_wait_time_l = 1; //長い足にかかる時間
-double target_max = 310; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
-double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
-double servo_iniposi=105; //サーボ初期角度
-double error_target=10;//PIDでの許容誤差
+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_;
-double pid_time=0.01;//PIDの割り込み時間
-double accel_max=0.01;//加速制限
+ 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();//ここがメインで走る記述
+};
+
-//定数
-#define Pi 3.14159265359 //円周率π
+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_;
+}
-//変数
-int target_num=0; //現在の目標プロット
-bool move_r=0,move_l=0; //
-double target_r_out=0,target_l_out=0;
-bool reach_target_r=0,reach_target_l=0;
-double posi_r=0,posi_l=0;
-double pre_out_r=0,pre_out_l=0;
-bool turn_mode=0;
-double turn_target=0;
+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_);
+}
@@ -61,685 +296,103 @@
void setup();
-void out_r(double);//モーターの出力
-void out_l(double);//モーターの出力
-void pid_r(double,double); //PID (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
-void pid_l(double,double); //PID (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
-void move(double,double); //(target_r,target_l)
-void up(int);
-void down(int);
-//void reset(); //脚位置の初期化
-void turn(double); //旋回動作
-void turn_2(double);
-void straight(double); //目的地まで直進
void wait_MR1();
-double get_dist();
-void output();
-void sanddune();
-void tussock();
-
-Ticker ticker;
+//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コントローラ生成 左右２つで、係数の変更はメンバ関数を用いて行う
+ //許容誤差設定
+ 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])};
- ticker.attach(&output,pid_time);
-
- /*while(1){
- printf("%f\r\n",degree0);
- }*/
-
- wait(3);
- /*down(long_r);
- down(long_l);
- wait(1);
- up(long_r);*/
-
- //straight(2800);
- //turn_2(45);
- //move(50,50);
- //move(190,190);
- sanddune();
-
- /*for(int i=0;i<15;i++){
- move(220,220);
- move(80,80);
- }*/
-
- /*for(int i=0;i<5;i++){
- move(300,300);
- move(30,30);
- }
- turn_2(45);
- for(int i=0;i<2;i++){
- move(300,300);
- move(30,30);
- }
- turn_2(0);
-
- for(int i=0;i<4;i++){
- move(300,300);
- move(30,30);
- }*/
-
-
-
- //sanddune();
-
- //move(300,300);
- //move(-300,-300);
+ //シリンダ初期設定
+ 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]);
- //wait(5);
- /*move(300,300);
- move(30,30);
- move(300,300);
- move(30,30);
- move(300,300);
- move(30,30);
- move(300,300);
- move(30,30);
- move(300,300);
- move(30,30);*/
+ robot.setLeg(&rightLeg, &leftLeg);
+ robot.setTickerTime(0.01); //モータ出力間隔 0.01
+ robot.setAirWaitTime(0.2); //シリンダを待つ時間
+ //初期位置は0これは内側が一番内側の時
- /*
- wait_MR1();
-
- for(int i=0; i<4; i++) {
- move(360,360);
- move(10,10);
- }
- turn(700);
- sanddune();
-
- move(360,360);
- move(10,10);
- move(10,10);
-
- turn(-700);
-
- tussock();*/
+ //example これで１サイクル
+ 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");
}
-double get_dist()
-{
- sensor.start();
- //wait_ms(50); //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
- //何ループも回す場合は不要。また、時間は適当だから調整が必要。
- return sensor.get_dist_cm();
-}
-
-
-
-//初期設定
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);
- switch_max.mode(PullUp);
- switch_min.mode(PullUp);
- switch_x.mode(PullUp);
- switch_y.mode(PullUp);
- switch_hand.mode(PullUp);
- for(int i=0; i<4; i++) {
- air[i]=1;
+
+ for (int i = 0; i < 4; i++)
+ {
+ air[i] = 1;
}
- air[4]=0;
- air[5]=0;
+ air[4] = 0;
+ air[5] = 0;
+
servo.init();
wait_us(1000);
- servo.set_degree(0,servo_iniposi);
-
- device.baud(115200);
- device.format(8,Serial::None,1);
- device.attach(dev_rx, Serial::RxIrq);
-
- wait(0.05);
- theta0=degree0;
-}
-int k=0;
-void output()
-{
- posi_r=1.0*ecgear*Pi*ec_r.getCount()/resolution;
- posi_l=1.0*ecgear*Pi*ec_l.getCount()/resolution;
-
- if(move_r) pid_r(target_r_out,error_target);
- else out_r(0);
- if(move_l) pid_l(target_l_out,error_target);
- else out_l(0);
-
- k++;
- if(k>50){
- printf("%f, %f, %f\r\n",posi_r,posi_l,degree0);
- k=0;
- }
-}
-
-
-//モーターの出力
-void out_r (double duty)
-{
- double dutylimit=0;
- if(turn_mode)dutylimit=dutylimit_turn;
- else dutylimit=dutylimit_nomal;
-
- if(duty > 0) { //入力duty比が正の場合
- if(duty-pre_out_r >accel_max)duty=pre_out_r+accel_max;
- //if(switch_max == 0) { //上限のリミットスイッチが押されていない場合
- if( fabs( duty ) < dutylimit ) { //制限値内
- motor_r_f = fabs(duty);
- motor_r_b = 0;
- } else { //制限値超
- motor_r_f = dutylimit;
- motor_r_b = 0;
- }
- /*} else { //上限のリミットスイッチが押されている場合
- motor_r_f = 0;
- motor_r_b = 0;
- }*/
-
- } else {//入力duty比が負の場合
- if(pre_out_r-duty >accel_max)duty=pre_out_r-accel_max;
- //if(switch_min == 0) { //下限のリミットスイッチが押されていない場合
- if( fabs(duty) < dutylimit) { //制限値内
- motor_r_f = 0;
- motor_r_b = fabs(duty);
- } else { //制限値超
- motor_r_f = 0;
- motor_r_b = dutylimit;
- }
- /*} else { //下限のリミットスイッチが押されている場合
- motor_r_f = 0;
- motor_r_b = 0;
- }*/
- }
- pre_out_r=duty;
- //if(k>49)printf("%f\r\n",dutylimit);
-}
-
-void out_l (double duty)
-{
- double dutylimit=0;
- if(turn_mode)dutylimit=dutylimit_turn;
- else dutylimit=dutylimit_nomal;
-
- if(duty > 0) { //入力duty比が正の場合
- if(duty-pre_out_l >accel_max)duty=pre_out_l+accel_max;
- //if(switch_max == 0) { //上限のリミットスイッチが押されていない場合
- if( fabs( duty ) < dutylimit ) { //制限値内
- motor_l_f = fabs(duty);
- motor_l_b = 0;
- } else { //制限値超
- motor_l_f = dutylimit;
- motor_l_b = 0;
- }
- /*} else { //上限のリミットスイッチが押されている場合
- motor_l_f = 0;
- motor_l_b = 0;
- }*/
-
- } else {//入力duty比が負の場合
- if(pre_out_l-duty >accel_max)duty=pre_out_l-accel_max;
- //if(switch_min == 0) { //下限のリミットスイッチが押されていない場合
- if( fabs(duty) < dutylimit) { //制限値内
- motor_l_f = 0;
- motor_l_b = fabs(duty);
- } else { //制限値超
- motor_l_f = 0;
- motor_l_b = dutylimit;
- }
- /*} else { //下限のリミットスイッチが押されている場合
- motor_l_f = 0;
- motor_l_b = 0;
- }*/
- }
- pre_out_l=duty;
-}
-
-
-double pile_r=0,distance_r_old=0,pile_l=0,distance_l_old=0;
-
-//PID制御
-void pid_r(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
-{
- double Kp_r,Ki_r,Kd_r;
- if(turn_mode){
- Kp_r=Kp_r_t;
- Ki_r=Ki_r_t;
- Kd_r=Kd_r_t;
- }else{
- Kp_r=Kp_r_n;
- Ki_r=Ki_r_n;
- Kd_r=Kd_r_n;
- }
- double deviation = target - posi_r;
- //printf("%f,%f\r\n",posi_r,deviation);
- pile_r += deviation;
- out_r(deviation * Kp_r - (posi_r - distance_r_old) / pid_time * Kd_r + pile_r * Ki_r * pid_time);
- distance_r_old = posi_r;
- if (fabs(deviation) < error) {
- out_r(0);
- pile_r=0;
- move_r=0;
- reach_target_r=1;
- }else if(turn_mode==1 && fabs(turn_target-degree0)<5){
- out_r(0);
- pile_r=0;
- move_r=0;
- reach_target_r=1;
- turn_mode=0;
- }
-}
-
-void pid_l(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
-{
- double Kp_l,Ki_l,Kd_l;
- if(turn_mode){
- Kp_l=Kp_l_t;
- Ki_l=Ki_l_t;
- Kd_l=Kd_l_t;
- }else{
- Kp_l=Kp_l_n;
- Ki_l=Ki_l_n;
- Kd_l=Kd_l_n;
- }
- double deviation = target - posi_l;
- pile_l += deviation;
- out_l(deviation * Kp_l - (posi_l - distance_l_old) / pid_time * Kd_l + pile_l * Ki_l * pid_time);
- distance_l_old = posi_l;
- if (fabs(deviation) < error) {
- out_l(0);
- pile_l=0;
- move_l=0;
- reach_target_l=1;
- }else if(turn_mode==1 && fabs(turn_target-degree0)<5){
- out_l(0);
- pile_l=0;
- move_l=0;
- reach_target_l=1;
- turn_mode=0;
- }
-}
-
-
-//脚を上げる
-void up(int i) // j=1 待ち時間あり
-{
- air[i]=0;
- //if(j)wait(leg_wait_time);
-}
-
-//脚を降ろす
-void down(int i)
-{
- air[i]=1;
- //if(j)wait(leg_wait_time);
-}
-
-
-
-//移動
-void move (double target_r,double target_l)
-{
- //pc.printf("x target=%f , posi=%f\r\n",x,linear_posi);
- int upleg_r=0,upleg_l=0;
- if(fabs(target_r)>target_max)target_r=target_max;
- else if(fabs(target_r)<target_min)target_r=target_min;
- if(target_r>0) {
- if(posi_r<target_r)upleg_r=r_o_s;
- else upleg_r=r_i_s;
- } else {
- if(posi_r>-target_r)upleg_r=r_o_s;
- else upleg_r=r_i_s;
- }
-
- if(fabs(target_l)>target_max)target_l=target_max;
- else if(fabs(target_l)<target_min)target_l=target_min;
- if(target_l>0) {
- if(posi_l>target_l)upleg_l=l_o_s;
- else upleg_l=l_i_s;
- } else {
- if(posi_l<-target_l)upleg_l=l_o_s;
- else upleg_l=l_i_s;
- }
- up(upleg_r);
- up(upleg_l);
-
- wait(leg_wait_time);
- target_r_out=fabs(target_r);
- target_l_out=fabs(target_l);
- reach_target_r=0;
- reach_target_l=0;
- move_r=1;
- move_l=1;
- //printf("%d\r\n",reach_target_r);
- /*while(reach_target_r==0 || reach_target_l==0){
- printf("%d\r\n",reach_target_r);
- };*/
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- //printf("%d\r\n",reach_target_r);
- }
- down(upleg_r);
- down(upleg_l);
- wait(leg_wait_time);
- reach_target_r=0;
- reach_target_l=0;
-}
-
-
+ servo.set_degree(0, 105); //初期角度105
-void straight(double distance) //とりあえず左右で位置をずらさない前提
-{
- bool reach=0;
- while(1) {
- if(distance>0) {
- if(target_max-posi_r > posi_r-target_min) {
- if(target_max-posi_r > distance) {
- move(posi_r+distance,posi_r+distance);
- reach=1;
- } else {
- distance-=target_max-posi_r;
- move(target_max,target_max);
- }
- } else {
- if(posi_r-target_min > distance) {
- move(posi_r-distance,posi_r-distance);
- reach=1;
- } else {
- distance-=posi_r-target_min;
- move(target_min,target_min);
- }
- }
- } else {
- if(target_max-posi_r > posi_r-target_min) {
- if(target_max-posi_r > -distance) {
- move(-posi_r+distance,-posi_r+distance);
- reach=1;
- } else {
- distance+=target_max-posi_r;
- move(-target_max,-target_max);
- }
- } else {
- if(posi_r-target_min> -distance) {
- move(-posi_r-distance,-posi_r-distance);
- reach=1;
- } else {
- distance+=posi_r-target_min;
- move(-target_min,-target_min);
- }
- }
- }
- if(reach==1)break;
- }
-}
-
-void turn(double distance) //理論計算の元気が出ないから脚の動かす量のみ distance>0:左回転
-{
- while(1) {
- if(distance>0) {
- if(target_max-posi_r > posi_r-target_min) {
- if(target_max-posi_r > distance) {
- move(posi_r+distance,-(posi_r+distance));
- break;
- } else {
- distance-=target_max-posi_r;
- move(target_max,-target_max);
- }
- } else {
- if(posi_r-target_min > distance) {
- move(-(posi_r-distance),posi_r-distance);
- break;
- } else {
- distance-=posi_r-target_min;
- move(-target_min,target_min);
- }
- }
- } else {
- if(target_max-posi_r > posi_r-target_min) {
- if(target_max-posi_r > -distance) {
- move(-(-posi_r+distance),-posi_r+distance);
- break;
- } else {
- distance+=target_max-posi_r;
- move(target_max,-target_max);
- }
- } else {
- if(posi_r-target_min> -distance) {
- move(-posi_r-distance,-(-posi_r-distance));
- break;
- } else {
- distance+=posi_r-target_min;
- move(-target_min,target_min);
- }
- }
- }
- }
-}
-
-
-double a=140,b=70;
-void turn_2(double target){
- turn_mode=1;
- turn_target=target;
- if(target-degree0>0){
- move(a,a);
- while(turn_mode){
- move(a+b,-(a+b));
- move(a,-a);
- }
- }else{
- move(a,a);
- while(turn_mode){
- move(-(a+b),a+b);
- move(-a,a);
- }
- }
-}
-
-void sanddune()
-{
- move(160,160);
- down(long_o);
- down(long_i);
- wait(1);
-
- up(long_i);
- wait(leg_wait_time_l);
- target_r_out=50;
- target_l_out=50;
- //target_l_out=270;
- reach_target_r=0;
- reach_target_l=0;
- move_r=1;
- move_l=1;
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- }
- down(long_i);
- //down(l_o_s);
- //down(r_i_s);
- wait(leg_wait_time_l);
- reach_target_r=0;
- reach_target_l=0;
+ device.baud(115200);
+ device.format(8, Serial::None, 1);
+ device.attach(dev_rx, Serial::RxIrq);
- up(long_o);
- wait(leg_wait_time_l);
- target_r_out=250;
- target_l_out=250;
- //target_l_out=70;
- reach_target_r=0;
- reach_target_l=0;
- move_r=1;
- move_l=1;
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- }
- down(long_o);
- wait(leg_wait_time_l);
- reach_target_r=0;
- reach_target_l=0;
-
- up(long_i);
- //up(l_o_s);
- //up(r_i_s);
- wait(leg_wait_time_l);
- target_r_out=160;
- target_l_out=160;
- reach_target_r=0;
- reach_target_l=0;
- move_r=1;
- move_l=1;
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- }
- down(long_i);
- wait(leg_wait_time_l);
- reach_target_r=0;
- reach_target_l=0;
-
- up(long_o);
- wait(leg_wait_time_l);
- target_r_out=220;
- target_l_out=220;
- //target_l_out=100;
- reach_target_r=0;
- reach_target_l=0;
- move_r=1;
- move_l=1;
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- }
- down(long_o);
- //down(r_o_s);
- //down(l_i_s);
- wait(leg_wait_time_l);
- reach_target_r=0;
- reach_target_l=0;
-
- up(long_i);
- wait(leg_wait_time_l);
- target_r_out=80;
- target_l_out=80;
- //target_l_out=240;
- reach_target_r=0;
- reach_target_l=0;
- move_r=1;
- move_l=1;
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- }
- down(long_i);
- wait(leg_wait_time_l);
- reach_target_r=0;
- reach_target_l=0;
-
- //up(r_o_s);
- //up(l_i_s);
- up(long_o);
- wait(leg_wait_time_l);
- target_r_out=0;
- target_l_out=0;
- reach_target_r=160;
- reach_target_l=160;
- move_r=1;
- move_l=1;
- while(reach_target_r==0 || reach_target_l==0){
- wait(0.01);
- }
- down(long_o);
- wait(leg_wait_time_l);
- reach_target_r=0;
- reach_target_l=0;
+ wait(0.05);
+ theta0 = degree0;
}
-
-void tussock()
-{
- for(int i=0; i<3; i++) {
- up(long_i);
- wait(leg_wait_time);
- target_r_out=360;
- target_l_out=360;
- while(reach_target_r==0 || reach_target_l==0);
- down(long_i);
- wait(leg_wait_time);
- reach_target_r=0;
- reach_target_l=0;
-
- up(long_o);
- wait(leg_wait_time);
- target_r_out=10;
- target_l_out=10;
- while(reach_target_r==0 || reach_target_l==0);
- down(long_o);
- wait(leg_wait_time);
- reach_target_r=0;
- reach_target_l=0;
- }
-}
-
-/*int k=0;
-//脚位置の初期化
-void reset()
-{
- up_x();
- while(switch_min == 0) {
- out(-0.2);
- k++;
- if(k>5000) {
- servo.set_degree(0,servo_iniposi);
- k=0;
- }
- }
- //pc.printf("%d\r\n",ec.getCount());
- ec.reset();
- out(0);
- down_x();
- //pc.printf("%d\r\n",ec.getCount());
-}*/
-
-
-
-
-
-/*void straight(double target)
-{
- while(1) {
- double xleg_posi=1.0*ecgear*Pi*ec.getCount()/resolution; //機体内のX脚位置
- double dist=target-linear_posi;
- pc.printf("leg=%f,dist=%f,linear_posi=%f\r\n",xleg_posi,dist,linear_posi);
- if(fabs(dist)<10) {
- break;
- } else if((dist>0 && dist<=(target_max-xleg_posi)) || (dist<0 && dist>=(target_min-xleg_posi))) {
- move_x(xleg_posi+dist,10);
- } else if(dist<0) {
- pc.printf("distance < 0");
- break;
- } else if(dist<=(target_max-target_min-10)) {
- if(dist<=(rail_len*0.5-target_min-10)) {
- move_y(dist+xleg_posi-rail_len*0.5,10);
- } else {
- move_y(target_min,10);
- }
- } else {
- if(xleg_posi<(rail_len*0.5))move_x(target_max,10);
- else move_y(target_min,10);
- }
- }
-}*/
-
void wait_MR1()
{
- while(switch_x==1) {
-
+ while (switch_x == 1)
+ {
}
//while(switch_hand==0);
//air_hand=1;
wait(1);
-
-}
\ No newline at end of file
+}