ROBOSTEP_5期 / Mbed 2 deprecated George_Master_BOTHMOVE

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed robot

main.cpp@14:1a3a673d85e3, 2019-05-02 (annotated)

Committer:
yuto17320508
Date:
Thu May 02 09:08:08 2019 +0000
Revision:
14:1a3a673d85e3
Parent:
13:29e71a2fd11e
Child:
15:1098bf926b5b
l

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:c1476d342c13 1 #include "mbed.h"
eri 0:c1476d342c13 2 #include "pin.h"
eri 0:c1476d342c13 3 #include "microinfinity.h"
eri 0:c1476d342c13 4
yuto17320508 4:81f01f93e502 5 //#define DEBUG_ON
yuto17320508 4:81f01f93e502 6
yuto17320508 4:81f01f93e502 7 #ifdef DEBUG_ON
yuto17320508 4:81f01f93e502 8 #define DEBUG(...) printf("" __VA_ARGS__);
yuto17320508 4:81f01f93e502 9 #else
yuto17320508 4:81f01f93e502 10 #define DEBUG(...)
yuto17320508 4:81f01f93e502 11 #endif
yuto17320508 4:81f01f93e502 12
yuto17320508 4:81f01f93e502 13 #define Pi 3.14159265359 //円周率π
yuto17320508 4:81f01f93e502 14
yuto17320508 14:1a3a673d85e3 15 enum WalkMode {
yuto17320508 12:9a5de6adae9c 16 BACK,
yuto17320508 12:9a5de6adae9c 17 STOP,
yuto17320508 12:9a5de6adae9c 18 FORWARD,
yuto17320508 12:9a5de6adae9c 19 };
yuto17320508 14:1a3a673d85e3 20 enum EVENT {
yuto17320508 12:9a5de6adae9c 21 NORMAL,
yuto17320508 12:9a5de6adae9c 22 GEREGE,
yuto17320508 14:1a3a673d85e3 23 GOAL,
yuto17320508 12:9a5de6adae9c 24 };
yuto17320508 4:81f01f93e502 25 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
yuto17320508 4:81f01f93e502 26
yuto17320508 4:81f01f93e502 27 class PIDcontroller //distanceをvalueに置き換えました
yuto17320508 4:81f01f93e502 28 {
yuto17320508 4:81f01f93e502 29 float Kp_, Ki_, Kd_, tolerance_, time_delta_;
yuto17320508 4:81f01f93e502 30 float pile_, value_old_, target_;
yuto17320508 4:81f01f93e502 31
yuto17320508 5:63462d696256 32 public:
yuto17320508 4:81f01f93e502 33 bool IsConvergence_; //収束したかどうか
yuto17320508 4:81f01f93e502 34 PIDcontroller(float Kp, float Ki, float Kd); //初期設定で係数を入力
yuto17320508 5:63462d696256 35 void setCoefficients(float Kp, float Ki, float Kd)
yuto17320508 5:63462d696256 36 {
yuto17320508 5:63462d696256 37 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 5:63462d696256 38 }; //係数を変更するときに使う
yuto17320508 5:63462d696256 39 void setTimeDelta(float delta)
yuto17320508 5:63462d696256 40 {
yuto17320508 5:63462d696256 41 time_delta_ = delta;
yuto17320508 5:63462d696256 42 };
yuto17320508 4:81f01f93e502 43 void setTarget(float target); //目標位置の設定
yuto17320508 5:63462d696256 44 void setTolerance(float tolerance)
yuto17320508 5:63462d696256 45 {
yuto17320508 5:63462d696256 46 tolerance_ = tolerance;
yuto17320508 5:63462d696256 47 }; //許容誤差の設定
yuto17320508 4:81f01f93e502 48 float calc(float nowVal); //現在位置と目標を比較してPID補正
yuto17320508 5:63462d696256 49 bool knowConvergence()
yuto17320508 5:63462d696256 50 {
yuto17320508 5:63462d696256 51 return IsConvergence_;
yuto17320508 5:63462d696256 52 }; //収束したかどうかを外部に伝える
yuto17320508 4:81f01f93e502 53 };
yuto17320508 4:81f01f93e502 54
yuto17320508 4:81f01f93e502 55 class Motor //PIDコントローラ、エンコーダを含むモータのクラス
yuto17320508 4:81f01f93e502 56 {
yuto17320508 4:81f01f93e502 57 PwmOut *pin_forward_, *pin_back_;
yuto17320508 4:81f01f93e502 58 Ec *ec_; //対応するエンコーダ
yuto17320508 4:81f01f93e502 59 float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
yuto17320508 4:81f01f93e502 60 int resolution_;
yuto17320508 5:63462d696256 61 public:
yuto17320508 4:81f01f93e502 62 Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
yuto17320508 5:63462d696256 63 void setDutyLimit(float limit)
yuto17320508 5:63462d696256 64 {
yuto17320508 5:63462d696256 65 duty_limit_ = limit;
yuto17320508 5:63462d696256 66 };
yuto17320508 14:1a3a673d85e3 67 float getDutyLimit()
yuto17320508 14:1a3a673d85e3 68 {
yuto17320508 14:1a3a673d85e3 69 return duty_limit_;
yuto17320508 14:1a3a673d85e3 70 };
yuto17320508 4:81f01f93e502 71 float getPosi(); //ポジをエンコーダから取得
yuto17320508 4:81f01f93e502 72 void calcDuty(PIDcontroller *pid); //Duty比を計算
yuto17320508 5:63462d696256 73 void setEncoder(Ec *ec)
yuto17320508 5:63462d696256 74 {
yuto17320508 5:63462d696256 75 ec_ = ec;
yuto17320508 5:63462d696256 76 }; //エンコーダを設定
yuto17320508 5:63462d696256 77 void setResolution(int reso)
yuto17320508 5:63462d696256 78 {
yuto17320508 5:63462d696256 79 resolution_ = reso;
yuto17320508 5:63462d696256 80 };
yuto17320508 4:81f01f93e502 81 void output(); //出力するだけ
yuto17320508 4:81f01f93e502 82 void output(float duty);
yuto17320508 4:81f01f93e502 83 };
yuto17320508 4:81f01f93e502 84
yuto17320508 4:81f01f93e502 85 class OneLeg //足の挙動を制御する
yuto17320508 4:81f01f93e502 86 {
yuto17320508 4:81f01f93e502 87 Motor *motor_;
yuto17320508 4:81f01f93e502 88 float target_pose_;
yuto17320508 4:81f01f93e502 89
yuto17320508 5:63462d696256 90 public:
yuto17320508 4:81f01f93e502 91 PIDcontroller *pid_;
yuto17320508 5:63462d696256 92 OneLeg() {};
yuto17320508 5:63462d696256 93 void setMotor(Motor *motor)
yuto17320508 5:63462d696256 94 {
yuto17320508 5:63462d696256 95 motor_ = motor;
yuto17320508 5:63462d696256 96 };
yuto17320508 5:63462d696256 97 void setPIDcontroller(PIDcontroller *pid)
yuto17320508 5:63462d696256 98 {
yuto17320508 5:63462d696256 99 pid_ = pid;
yuto17320508 5:63462d696256 100 };
yuto17320508 4:81f01f93e502 101 void setTargetPose(float target_pose);
yuto17320508 4:81f01f93e502 102 void actMotor();//モータ出力
yuto17320508 4:81f01f93e502 103 };
yuto17320508 4:81f01f93e502 104
yuto17320508 4:81f01f93e502 105 class Robot
yuto17320508 4:81f01f93e502 106 {
yuto17320508 4:81f01f93e502 107 float ticker_time_, air_wait_time_;
yuto17320508 4:81f01f93e502 108 OneLeg *Leg1_, *Leg2_;
yuto17320508 4:81f01f93e502 109 Timer timer;
yuto17320508 4:81f01f93e502 110
yuto17320508 5:63462d696256 111 public:
yuto17320508 5:63462d696256 112 Robot()
yuto17320508 5:63462d696256 113 {
yuto17320508 5:63462d696256 114 timer.reset();
yuto17320508 5:63462d696256 115 timer.start();
yuto17320508 5:63462d696256 116 };
yuto17320508 4:81f01f93e502 117 void setLeg(OneLeg *Leg1_, OneLeg *Leg2_);
yuto17320508 4:81f01f93e502 118 void setTickerTime(float ticker_time);
yuto17320508 4:81f01f93e502 119 void run();//ここがメインで走る記述
yuto17320508 4:81f01f93e502 120 };
yuto17320508 4:81f01f93e502 121
yuto17320508 4:81f01f93e502 122
yuto17320508 4:81f01f93e502 123
yuto17320508 4:81f01f93e502 124 PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
yuto17320508 4:81f01f93e502 125 {
yuto17320508 4:81f01f93e502 126 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 4:81f01f93e502 127 DEBUG("set Kp:%.3f KI:%.3f Kd:%.3f \n\r", Kp_, Ki_, Kd_);
yuto17320508 4:81f01f93e502 128 IsConvergence_=true;
yuto17320508 4:81f01f93e502 129 }
yuto17320508 4:81f01f93e502 130 void PIDcontroller::setTarget(float target)
yuto17320508 4:81f01f93e502 131 {
yuto17320508 5:63462d696256 132 if (IsConvergence_) { //収束時のみ変更可能
yuto17320508 4:81f01f93e502 133 target_ = target;
yuto17320508 4:81f01f93e502 134 DEBUG("set Target: %.3f\n\r", target_);
yuto17320508 4:81f01f93e502 135 IsConvergence_ = false;
yuto17320508 5:63462d696256 136 } else {
yuto17320508 4:81f01f93e502 137 DEBUG("error: setTarget permission denied!\n");
yuto17320508 4:81f01f93e502 138 }
yuto17320508 4:81f01f93e502 139 }
yuto17320508 4:81f01f93e502 140 float PIDcontroller::calc(float nowVal)
yuto17320508 4:81f01f93e502 141 {
yuto17320508 4:81f01f93e502 142 float out = 0;
yuto17320508 4:81f01f93e502 143 //PID計算ここで行う
yuto17320508 4:81f01f93e502 144 float deviation = target_ - nowVal; //目標との差分
yuto17320508 4:81f01f93e502 145 pile_ += deviation; //積分用に和を取る
yuto17320508 4:81f01f93e502 146 out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
yuto17320508 4:81f01f93e502 147 value_old_ = nowVal; //今のデータを保存
yuto17320508 4:81f01f93e502 148 //
yuto17320508 5:63462d696256 149 if (fabs(deviation) < tolerance_) { //収束した場合
yuto17320508 4:81f01f93e502 150 DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
yuto17320508 4:81f01f93e502 151 out = 0;
yuto17320508 4:81f01f93e502 152 pile_ = 0;
yuto17320508 4:81f01f93e502 153 value_old_ = 0;
yuto17320508 4:81f01f93e502 154 IsConvergence_ = true;
yuto17320508 4:81f01f93e502 155 }
yuto17320508 4:81f01f93e502 156 return out;
yuto17320508 4:81f01f93e502 157 }
yuto17320508 4:81f01f93e502 158
yuto17320508 4:81f01f93e502 159 Motor::Motor(PwmOut *forward, PwmOut *back)
yuto17320508 4:81f01f93e502 160 {
yuto17320508 4:81f01f93e502 161 pin_forward_ = forward;
yuto17320508 4:81f01f93e502 162 pin_back_ = back;
yuto17320508 4:81f01f93e502 163 }
yuto17320508 4:81f01f93e502 164 float Motor::getPosi()
yuto17320508 4:81f01f93e502 165 {
yuto17320508 4:81f01f93e502 166 float posi = 2.0*180*((float)(ec_->getCount)()/(float)resolution_);
yuto17320508 5:63462d696256 167
yuto17320508 4:81f01f93e502 168 //DEBUG("value :%d :%d\n\r", (ec_->getCount)(),resolution_);
yuto17320508 4:81f01f93e502 169 DEBUG("posi is %.4f\n\r",posi);
yuto17320508 4:81f01f93e502 170 return posi;
yuto17320508 4:81f01f93e502 171 }
yuto17320508 4:81f01f93e502 172 void Motor::calcDuty(PIDcontroller *pid)
yuto17320508 5:63462d696256 173 {
yuto17320508 4:81f01f93e502 174 duty_ = pid->calc(getPosi());
yuto17320508 4:81f01f93e502 175 DEBUG("duty is %.4f\n\r",duty_);
yuto17320508 4:81f01f93e502 176 }
yuto17320508 4:81f01f93e502 177 void Motor::output()
yuto17320508 4:81f01f93e502 178 {
yuto17320508 4:81f01f93e502 179 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 4:81f01f93e502 180 //加速度が一定値を超えたら変更加える
yuto17320508 5:63462d696256 181 if (duty_ > 0) {
yuto17320508 12:9a5de6adae9c 182 //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
yuto17320508 4:81f01f93e502 183 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 184 pin_forward_->write(output_duty);
yuto17320508 4:81f01f93e502 185 pin_back_->write(0);
yuto17320508 4:81f01f93e502 186 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 5:63462d696256 187 } else {
yuto17320508 12:9a5de6adae9c 188 //if (pre_duty_ - duty_ > accel_max)
yuto17320508 12:9a5de6adae9c 189 // duty_ = pre_duty_ - accel_max;
yuto17320508 4:81f01f93e502 190 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 191 pin_forward_->write(0);
yuto17320508 4:81f01f93e502 192 pin_back_->write(output_duty);
yuto17320508 4:81f01f93e502 193 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 4:81f01f93e502 194 }
yuto17320508 4:81f01f93e502 195 pre_duty_ = duty_;
yuto17320508 4:81f01f93e502 196 }
yuto17320508 4:81f01f93e502 197 void Motor::output(float duty)
yuto17320508 4:81f01f93e502 198 {
yuto17320508 4:81f01f93e502 199 duty_ = duty;
yuto17320508 4:81f01f93e502 200 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 4:81f01f93e502 201 //加速度が一定値を超えたら変更加える
yuto17320508 5:63462d696256 202 if (duty_ > 0) {
yuto17320508 4:81f01f93e502 203 //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
yuto17320508 4:81f01f93e502 204 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 205 pin_forward_->write(output_duty);
yuto17320508 4:81f01f93e502 206 pin_back_->write(0);
yuto17320508 4:81f01f93e502 207 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 5:63462d696256 208 } else {
yuto17320508 4:81f01f93e502 209 //if (pre_duty_ - duty_ > accel_max)
yuto17320508 4:81f01f93e502 210 // duty_ = pre_duty_ - accel_max;
yuto17320508 4:81f01f93e502 211 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 212 pin_forward_->write(0);
yuto17320508 4:81f01f93e502 213 pin_back_->write(output_duty);
yuto17320508 4:81f01f93e502 214 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 4:81f01f93e502 215 }
yuto17320508 4:81f01f93e502 216 pre_duty_ = duty_;
yuto17320508 4:81f01f93e502 217 }
yuto17320508 4:81f01f93e502 218
yuto17320508 4:81f01f93e502 219 void OneLeg::setTargetPose(float target_pose)
yuto17320508 4:81f01f93e502 220 {
yuto17320508 4:81f01f93e502 221 target_pose_ = target_pose;
yuto17320508 4:81f01f93e502 222 //PIDにtargetを送る
yuto17320508 4:81f01f93e502 223 pid_->setTarget(target_pose_);
yuto17320508 4:81f01f93e502 224 }
yuto17320508 4:81f01f93e502 225 void OneLeg::actMotor()
yuto17320508 4:81f01f93e502 226 {
yuto17320508 4:81f01f93e502 227 motor_->calcDuty(pid_);
yuto17320508 4:81f01f93e502 228 motor_->output();
yuto17320508 4:81f01f93e502 229 }
yuto17320508 4:81f01f93e502 230
yuto17320508 4:81f01f93e502 231
yuto17320508 4:81f01f93e502 232
yuto17320508 4:81f01f93e502 233 void Robot::setLeg(OneLeg *Leg1, OneLeg *Leg2)
yuto17320508 4:81f01f93e502 234 {
yuto17320508 4:81f01f93e502 235 Leg1_ = Leg1;
yuto17320508 4:81f01f93e502 236 Leg2_ = Leg2;
yuto17320508 4:81f01f93e502 237 }
yuto17320508 4:81f01f93e502 238 void Robot::setTickerTime(float ticker_time)
yuto17320508 4:81f01f93e502 239 {
yuto17320508 4:81f01f93e502 240 ticker_time_ = ticker_time;
yuto17320508 4:81f01f93e502 241 Leg1_->pid_->setTimeDelta(ticker_time_);
yuto17320508 4:81f01f93e502 242 Leg2_->pid_->setTimeDelta(ticker_time_);
yuto17320508 4:81f01f93e502 243 }
yuto17320508 4:81f01f93e502 244 void Robot::run()
yuto17320508 4:81f01f93e502 245 {
yuto17320508 5:63462d696256 246 while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) { //片方が収束していない時*/
yuto17320508 4:81f01f93e502 247 //ticker_time毎にモータに出力する
yuto17320508 4:81f01f93e502 248 float time_s = timer.read();
yuto17320508 4:81f01f93e502 249 Leg1_->actMotor();
yuto17320508 4:81f01f93e502 250 Leg2_->actMotor();
yuto17320508 4:81f01f93e502 251 float rest_time_s = ticker_time_ - (timer.read() - time_s);
yuto17320508 4:81f01f93e502 252 //ticker_timeまで待機
yuto17320508 5:63462d696256 253 if (rest_time_s > 0) {
yuto17320508 4:81f01f93e502 254 wait(rest_time_s);
yuto17320508 4:81f01f93e502 255 DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
yuto17320508 4:81f01f93e502 256 }
yuto17320508 5:63462d696256 257
yuto17320508 4:81f01f93e502 258 else //時間が足りない場合警告
yuto17320508 4:81f01f93e502 259 printf("error: restTime not enough\n\r");
yuto17320508 4:81f01f93e502 260 DEBUG("loop end\n\r")
yuto17320508 4:81f01f93e502 261 }
yuto17320508 4:81f01f93e502 262
yuto17320508 4:81f01f93e502 263 }
yuto17320508 4:81f01f93e502 264
eri 0:c1476d342c13 265
eri 0:c1476d342c13 266
eri 0:c1476d342c13 267 ////////////関数
yuto17320508 4:81f01f93e502 268 void reset();
eri 0:c1476d342c13 269 void setup();
yuto17320508 10:a335588b9ef0 270 void set_gyro();
yuto17320508 10:a335588b9ef0 271 double get_dist_forward();
yuto17320508 10:a335588b9ef0 272 double get_dist_back();
yuto17320508 12:9a5de6adae9c 273 void can_send(int mode, float duty);
yuto17320508 12:9a5de6adae9c 274 void straight();
yuto17320508 12:9a5de6adae9c 275 void turnLeft();
yuto17320508 12:9a5de6adae9c 276 void curveLeft();
yuto17320508 12:9a5de6adae9c 277 void turnRight();
yuto17320508 12:9a5de6adae9c 278 void curveRight();
yuto17320508 13:29e71a2fd11e 279 void turn(float target_degree);//回転角, 収束許容誤差
yuto17320508 10:a335588b9ef0 280 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
eri 0:c1476d342c13 281
yuto17320508 12:9a5de6adae9c 282 void wait_gerege();
yuto17320508 12:9a5de6adae9c 283
eri 0:c1476d342c13 284 ////////////定数
eri 0:c1476d342c13 285
eri 0:c1476d342c13 286 ////////////変数
yuto17320508 14:1a3a673d85e3 287 int hand_mode=NORMAL;
kageyuta 1:86c4c38abe40 288
yuto17320508 4:81f01f93e502 289 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
yuto17320508 4:81f01f93e502 290 //しかし変更を多々行うためポインタ渡しにしてある
yuto17320508 4:81f01f93e502 291 //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
yuto17320508 4:81f01f93e502 292 PIDcontroller pid_lo(0.01, 0.000, 0.000);
yuto17320508 4:81f01f93e502 293 PIDcontroller pid_li(0.01, 0.000, 0.000); //Kp.Ki,Kd
yuto17320508 4:81f01f93e502 294 Motor motor_lo(&motor_lo_f, &motor_lo_b),
yuto17320508 5:63462d696256 295 motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
yuto17320508 4:81f01f93e502 296 OneLeg leg_lo, leg_li;
yuto17320508 4:81f01f93e502 297 Robot robot;
eri 0:c1476d342c13 298
yuto17320508 10:a335588b9ef0 299 int step_num_l, step_num_r;
yuto17320508 10:a335588b9ef0 300
yuto17320508 10:a335588b9ef0 301 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
yuto17320508 10:a335588b9ef0 302 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
yuto17320508 10:a335588b9ef0 303 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
kageyuta 1:86c4c38abe40 304 int main()
kageyuta 1:86c4c38abe40 305 {
eri 0:c1476d342c13 306 setup();
yuto17320508 14:1a3a673d85e3 307
yuto17320508 4:81f01f93e502 308 pid_lo.setTolerance(10);
yuto17320508 4:81f01f93e502 309 pid_li.setTolerance(10);
yuto17320508 4:81f01f93e502 310 motor_lo.setEncoder(&ec_lo);
yuto17320508 4:81f01f93e502 311 motor_lo.setResolution(1000);
yuto17320508 4:81f01f93e502 312 motor_li.setEncoder(&ec_li);
yuto17320508 7:cff25545558f 313 motor_li.setResolution(600);
yuto17320508 4:81f01f93e502 314 leg_lo.setMotor(&motor_lo);
yuto17320508 4:81f01f93e502 315 leg_lo.setPIDcontroller(&pid_lo);
yuto17320508 4:81f01f93e502 316 leg_li.setMotor(&motor_li);
yuto17320508 4:81f01f93e502 317 leg_li.setPIDcontroller(&pid_li);
yuto17320508 4:81f01f93e502 318 robot.setLeg(&leg_lo, &leg_li);
yuto17320508 4:81f01f93e502 319 robot.setTickerTime(0.01); //モータ出力間隔 0.01
kageyuta 11:a6fadff7cc78 320 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 9:e248986c8423 321 motor_li.setDutyLimit(0.5);
yuto17320508 12:9a5de6adae9c 322 printf("reset standby\n\r");
kageyuta 2:55c616d2e0fe 323 reset();
yuto17320508 4:81f01f93e502 324 printf("bus standby\n\r");
yuto17320508 14:1a3a673d85e3 325
yuto17320508 5:63462d696256 326 while(1) {
yuto17320508 5:63462d696256 327 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 328 }
yuto17320508 4:81f01f93e502 329 printf("bus is %d\n\r", bus_in.read());
yuto17320508 14:1a3a673d85e3 330
yuto17320508 12:9a5de6adae9c 331 wait(0.5);
yuto17320508 14:1a3a673d85e3 332
yuto17320508 12:9a5de6adae9c 333 straight();
yuto17320508 14:1a3a673d85e3 334
yuto17320508 12:9a5de6adae9c 335 wait_gerege();
yuto17320508 14:1a3a673d85e3 336
yuto17320508 12:9a5de6adae9c 337 hand_mode = GEREGE;
yuto17320508 14:1a3a673d85e3 338
yuto17320508 10:a335588b9ef0 339 set_gyro();
yuto17320508 4:81f01f93e502 340 //Sample
yuto17320508 10:a335588b9ef0 341 //スタート直進
yuto17320508 10:a335588b9ef0 342 printf("dist:%.3f\r\n", get_dist_forward());
yuto17320508 14:1a3a673d85e3 343
yuto17320508 13:29e71a2fd11e 344 for(int i=0;i<3;++i){
yuto17320508 13:29e71a2fd11e 345 while(get_dist_back() < 280)
yuto17320508 13:29e71a2fd11e 346 {
yuto17320508 13:29e71a2fd11e 347 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 348 //wait(0.01);
yuto17320508 14:1a3a673d85e3 349 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 13:29e71a2fd11e 350 }
yuto17320508 3:7a608fbd3bcd 351 }
yuto17320508 12:9a5de6adae9c 352 //printf("back dist:%.3f\r\n", get_dist_back());
yuto17320508 10:a335588b9ef0 353 //段差前旋回
yuto17320508 12:9a5de6adae9c 354 motor_lo.setDutyLimit(0.4);//0.5
yuto17320508 12:9a5de6adae9c 355 motor_li.setDutyLimit(0.4);
yuto17320508 13:29e71a2fd11e 356 turn(40.0);
yuto17320508 10:a335588b9ef0 357 //段差乗り越え
yuto17320508 12:9a5de6adae9c 358 for(int i= 0;i<5;++i) straight();
yuto17320508 12:9a5de6adae9c 359 while(get_dist_back() > 40) straight();
yuto17320508 10:a335588b9ef0 360 //段差後旋回
yuto17320508 12:9a5de6adae9c 361 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 13:29e71a2fd11e 362 turn(90.0);
yuto17320508 12:9a5de6adae9c 363 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 10:a335588b9ef0 364 //前進
yuto17320508 12:9a5de6adae9c 365 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 12:9a5de6adae9c 366 motor_li.setDutyLimit(0.5);
yuto17320508 13:29e71a2fd11e 367 for(int i=0;i<3;++i)
yuto17320508 12:9a5de6adae9c 368 {
yuto17320508 14:1a3a673d85e3 369 while(get_dist_forward() > 65)
yuto17320508 13:29e71a2fd11e 370 {
yuto17320508 13:29e71a2fd11e 371 straightAndInfinity(90.0, 5.0);
yuto17320508 13:29e71a2fd11e 372 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 13:29e71a2fd11e 373 }
yuto17320508 12:9a5de6adae9c 374 }
yuto17320508 10:a335588b9ef0 375 //ロープ前旋回
yuto17320508 13:29e71a2fd11e 376 printf("before roop deg:%.3f\n\r",degree0);
yuto17320508 13:29e71a2fd11e 377 turn(0);
yuto17320508 14:1a3a673d85e3 378
yuto17320508 10:a335588b9ef0 379 //ロープ
yuto17320508 13:29e71a2fd11e 380 while(mode4.read() == 0)
yuto17320508 13:29e71a2fd11e 381 {
yuto17320508 14:1a3a673d85e3 382
yuto17320508 13:29e71a2fd11e 383 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 384 }
yuto17320508 14:1a3a673d85e3 385
yuto17320508 14:1a3a673d85e3 386 printf("go to uhai deg:%.3f forward dist:%.3f \n\r",degree0,get_dist_forward());
yuto17320508 13:29e71a2fd11e 387 for(int i=0;i<3;++i)
kageyuta 11:a6fadff7cc78 388 {
yuto17320508 14:1a3a673d85e3 389 while(get_dist_forward() > 65)//65
yuto17320508 13:29e71a2fd11e 390 {
yuto17320508 13:29e71a2fd11e 391 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 392 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 13:29e71a2fd11e 393 }
kageyuta 11:a6fadff7cc78 394 }
yuto17320508 14:1a3a673d85e3 395 printf("turn");
yuto17320508 14:1a3a673d85e3 396 turn(-90);
yuto17320508 14:1a3a673d85e3 397
yuto17320508 12:9a5de6adae9c 398
yuto17320508 14:1a3a673d85e3 399 motor_lo.setDutyLimit(0.2);//0.5
yuto17320508 14:1a3a673d85e3 400 motor_li.setDutyLimit(0.2);
yuto17320508 14:1a3a673d85e3 401
yuto17320508 14:1a3a673d85e3 402 for(int i=0; i<15; ++i)straightAndInfinity(0, 5);
yuto17320508 14:1a3a673d85e3 403 printf("wall standby");
yuto17320508 14:1a3a673d85e3 404 while(get_dist_back() < 250) {
yuto17320508 14:1a3a673d85e3 405 straightAndInfinity(0, 5);
yuto17320508 14:1a3a673d85e3 406 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 14:1a3a673d85e3 407 }
yuto17320508 14:1a3a673d85e3 408 for(int i=0; i<2; ++i) {
yuto17320508 14:1a3a673d85e3 409 while(get_dist_forward() > 65) {
yuto17320508 14:1a3a673d85e3 410 straightAndInfinity(0, 5);
yuto17320508 14:1a3a673d85e3 411 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 14:1a3a673d85e3 412 }
yuto17320508 14:1a3a673d85e3 413 }
yuto17320508 12:9a5de6adae9c 414 hand_mode = GOAL;
yuto17320508 12:9a5de6adae9c 415 straight();
yuto17320508 13:29e71a2fd11e 416 printf("uhai!!!!!!!!!!!!!!!");
yuto17320508 14:1a3a673d85e3 417
yuto17320508 14:1a3a673d85e3 418
yuto17320508 10:a335588b9ef0 419 }
yuto17320508 13:29e71a2fd11e 420 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
yuto17320508 10:a335588b9ef0 421 {
yuto17320508 14:1a3a673d85e3 422 if(target_degree - degree0 > 0) {
yuto17320508 10:a335588b9ef0 423 while(target_degree - degree0 > 0)
yuto17320508 14:1a3a673d85e3 424 turnLeft();
yuto17320508 14:1a3a673d85e3 425 } else {
yuto17320508 10:a335588b9ef0 426 while(target_degree - degree0 < 0)
yuto17320508 14:1a3a673d85e3 427 turnRight();
yuto17320508 10:a335588b9ef0 428 }
yuto17320508 13:29e71a2fd11e 429 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 10:a335588b9ef0 430 }
yuto17320508 10:a335588b9ef0 431 void straightAndInfinity(float target_degree, float tolerance_degree)
yuto17320508 10:a335588b9ef0 432 {
yuto17320508 12:9a5de6adae9c 433 if(target_degree - degree0 > tolerance_degree) curveLeft();
yuto17320508 12:9a5de6adae9c 434 else if(degree0 - target_degree > tolerance_degree) curveRight();
yuto17320508 12:9a5de6adae9c 435 else straight();
yuto17320508 5:63462d696256 436 }
yuto17320508 12:9a5de6adae9c 437 void straight()
yuto17320508 5:63462d696256 438 {
yuto17320508 12:9a5de6adae9c 439 can_send(FORWARD, motor_lo.getDutyLimit());
yuto17320508 5:63462d696256 440 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 5:63462d696256 441 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 5:63462d696256 442 robot.run();
yuto17320508 4:81f01f93e502 443 motor_lo_f.write(0);
yuto17320508 4:81f01f93e502 444 motor_lo_b.write(0);
yuto17320508 4:81f01f93e502 445 motor_li_f.write(0);
yuto17320508 4:81f01f93e502 446 motor_li_b.write(0);
yuto17320508 5:63462d696256 447 while(1) {
yuto17320508 5:63462d696256 448 if(bus_in.read() == 1) break;
yuto17320508 5:63462d696256 449 }
yuto17320508 7:cff25545558f 450 step_num_l++;
yuto17320508 7:cff25545558f 451 step_num_r++;
yuto17320508 5:63462d696256 452 }
yuto17320508 12:9a5de6adae9c 453 void turnLeft()
yuto17320508 5:63462d696256 454 {
yuto17320508 12:9a5de6adae9c 455 can_send(FORWARD, motor_lo.getDutyLimit());
yuto17320508 5:63462d696256 456 leg_lo.setTargetPose(360+(step_num_l-2)*180);
yuto17320508 5:63462d696256 457 leg_li.setTargetPose(180+(step_num_l-2)*180);
yuto17320508 5:63462d696256 458 robot.run();
yuto17320508 5:63462d696256 459 motor_lo_f.write(0);
yuto17320508 5:63462d696256 460 motor_lo_b.write(0);
yuto17320508 5:63462d696256 461 motor_li_f.write(0);
yuto17320508 5:63462d696256 462 motor_li_b.write(0);
yuto17320508 5:63462d696256 463 while(1) {
yuto17320508 5:63462d696256 464 if(bus_in.read() == 1) break;
yuto17320508 5:63462d696256 465 }
yuto17320508 5:63462d696256 466 step_num_r++;
yuto17320508 5:63462d696256 467 step_num_l--;
yuto17320508 5:63462d696256 468 }
yuto17320508 12:9a5de6adae9c 469 void curveLeft()
yuto17320508 10:a335588b9ef0 470 {
yuto17320508 12:9a5de6adae9c 471 can_send(FORWARD, motor_lo.getDutyLimit());
yuto17320508 10:a335588b9ef0 472 leg_lo.setTargetPose(360+(step_num_l-1)*180);
yuto17320508 10:a335588b9ef0 473 leg_li.setTargetPose(180+(step_num_l-1)*180);
yuto17320508 10:a335588b9ef0 474 robot.run();
yuto17320508 10:a335588b9ef0 475 motor_lo_f.write(0);
yuto17320508 10:a335588b9ef0 476 motor_lo_b.write(0);
yuto17320508 10:a335588b9ef0 477 motor_li_f.write(0);
yuto17320508 10:a335588b9ef0 478 motor_li_b.write(0);
yuto17320508 10:a335588b9ef0 479 while(1) {
yuto17320508 10:a335588b9ef0 480 if(bus_in.read() == 1) break;
yuto17320508 10:a335588b9ef0 481 }
yuto17320508 10:a335588b9ef0 482 step_num_r++;
yuto17320508 10:a335588b9ef0 483 }
yuto17320508 12:9a5de6adae9c 484 void turnRight()
yuto17320508 5:63462d696256 485 {
yuto17320508 12:9a5de6adae9c 486 can_send(BACK, motor_lo.getDutyLimit());
yuto17320508 8:ded0354412ae 487 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 8:ded0354412ae 488 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 5:63462d696256 489 robot.run();
yuto17320508 5:63462d696256 490 motor_lo_f.write(0);
yuto17320508 5:63462d696256 491 motor_lo_b.write(0);
yuto17320508 5:63462d696256 492 motor_li_f.write(0);
yuto17320508 5:63462d696256 493 motor_li_b.write(0);
yuto17320508 5:63462d696256 494 while(1) {
yuto17320508 5:63462d696256 495 if(bus_in.read() == 1) break;
yuto17320508 5:63462d696256 496 }
yuto17320508 8:ded0354412ae 497 step_num_r--;
yuto17320508 8:ded0354412ae 498 step_num_l++;
eri 0:c1476d342c13 499 }
yuto17320508 12:9a5de6adae9c 500 void curveRight()
yuto17320508 10:a335588b9ef0 501 {
yuto17320508 12:9a5de6adae9c 502 can_send(STOP, motor_lo.getDutyLimit());
yuto17320508 10:a335588b9ef0 503 leg_lo.setTargetPose(360+step_num_l*180);
yuto17320508 10:a335588b9ef0 504 leg_li.setTargetPose(180+step_num_l*180);
yuto17320508 10:a335588b9ef0 505 robot.run();
yuto17320508 10:a335588b9ef0 506 motor_lo_f.write(0);
yuto17320508 10:a335588b9ef0 507 motor_lo_b.write(0);
yuto17320508 10:a335588b9ef0 508 motor_li_f.write(0);
yuto17320508 10:a335588b9ef0 509 motor_li_b.write(0);
yuto17320508 10:a335588b9ef0 510 while(1) {
yuto17320508 10:a335588b9ef0 511 if(bus_in.read() == 1) break;
yuto17320508 10:a335588b9ef0 512 }
yuto17320508 10:a335588b9ef0 513 step_num_l++;
yuto17320508 10:a335588b9ef0 514 }
eri 0:c1476d342c13 515
eri 0:c1476d342c13 516
kageyuta 1:86c4c38abe40 517 void setup()
kageyuta 1:86c4c38abe40 518 {
eri 0:c1476d342c13 519 can1.frequency(1000000);
eri 0:c1476d342c13 520 motor_lo_f.period_us(100);
eri 0:c1476d342c13 521 motor_lo_b.period_us(100);
eri 0:c1476d342c13 522 motor_li_f.period_us(100);
eri 0:c1476d342c13 523 motor_li_b.period_us(100);
kageyuta 1:86c4c38abe40 524
eri 0:c1476d342c13 525 hand.mode(PullUp);
kageyuta 2:55c616d2e0fe 526 switch_lo.mode(PullUp);
kageyuta 2:55c616d2e0fe 527 switch_li.mode(PullUp);
yuto17320508 13:29e71a2fd11e 528 mode4.mode(PullUp);
kageyuta 1:86c4c38abe40 529
yuto17320508 10:a335588b9ef0 530 }
eri 0:c1476d342c13 531
yuto17320508 10:a335588b9ef0 532 void set_gyro()
yuto17320508 10:a335588b9ef0 533 {
eri 0:c1476d342c13 534 device.baud(115200);
eri 0:c1476d342c13 535 device.format(8,Serial::None,1);
eri 0:c1476d342c13 536 device.attach(dev_rx, Serial::RxIrq);
eri 0:c1476d342c13 537 wait(0.05);
eri 0:c1476d342c13 538 theta0=degree0;
eri 0:c1476d342c13 539 check_gyro();
eri 0:c1476d342c13 540 }
eri 0:c1476d342c13 541
eri 0:c1476d342c13 542
eri 0:c1476d342c13 543 //////////////////////////////////////can
yuto17320508 12:9a5de6adae9c 544 void can_send(int mode, float duty)
eri 0:c1476d342c13 545 {
yuto17320508 12:9a5de6adae9c 546 char data[2]= {0};
yuto17320508 12:9a5de6adae9c 547 int send=mode * 10 + (int)(duty * 10.0);
yuto17320508 12:9a5de6adae9c 548 //printf("duty is %.3f\n\r",duty);
yuto17320508 12:9a5de6adae9c 549 data[0]=send & 0b11111111;
yuto17320508 12:9a5de6adae9c 550 data[1]=hand_mode;
kageyuta 1:86c4c38abe40 551
yuto17320508 12:9a5de6adae9c 552 if(can1.write(CANMessage(0,data,2)))led4=1;
eri 0:c1476d342c13 553 else led4=0;
kageyuta 1:86c4c38abe40 554 }
kageyuta 2:55c616d2e0fe 555 void reset()
kageyuta 2:55c616d2e0fe 556 {
kageyuta 2:55c616d2e0fe 557 while(switch_lo.read()) {
kageyuta 6:fe9fa8c2e6f9 558 motor_lo.output(0.3);
kageyuta 2:55c616d2e0fe 559 }
yuto17320508 5:63462d696256 560 ec_lo.reset();
yuto17320508 5:63462d696256 561 motor_lo.output(0.0);
kageyuta 2:55c616d2e0fe 562 while(switch_li.read()) {
kageyuta 6:fe9fa8c2e6f9 563 motor_li.output(0.3);
kageyuta 2:55c616d2e0fe 564 }
yuto17320508 5:63462d696256 565 ec_li.reset();
yuto17320508 5:63462d696256 566 motor_li.output(0.0);
kageyuta 1:86c4c38abe40 567 }
yuto17320508 10:a335588b9ef0 568 double get_dist_back()
yuto17320508 10:a335588b9ef0 569 {
yuto17320508 10:a335588b9ef0 570 sensor_back.start();
yuto17320508 14:1a3a673d85e3 571 wait_ms(10);//10 //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
yuto17320508 10:a335588b9ef0 572 //何ループも回す場合は不要。また、時間は適当だから調整が必要。
yuto17320508 14:1a3a673d85e3 573 float dist = sensor_back.get_dist_cm();
yuto17320508 14:1a3a673d85e3 574 if(dist < 0.1) dist = 2000.0;
yuto17320508 14:1a3a673d85e3 575 return dist;
yuto17320508 10:a335588b9ef0 576 }
yuto17320508 10:a335588b9ef0 577 double get_dist_forward()
yuto17320508 10:a335588b9ef0 578 {
kageyuta 11:a6fadff7cc78 579 sensor_forward.start();
yuto17320508 14:1a3a673d85e3 580 wait_ms(10);//10
yuto17320508 14:1a3a673d85e3 581 float dist = sensor_forward.get_dist_cm();
yuto17320508 14:1a3a673d85e3 582 if(dist < 0.1) dist = 2000.0;
yuto17320508 14:1a3a673d85e3 583 return dist;
yuto17320508 10:a335588b9ef0 584 }
yuto17320508 12:9a5de6adae9c 585
yuto17320508 12:9a5de6adae9c 586 void wait_gerege()
yuto17320508 12:9a5de6adae9c 587 {
yuto17320508 12:9a5de6adae9c 588 int i = 0;
yuto17320508 14:1a3a673d85e3 589 while(i!=100) {
yuto17320508 14:1a3a673d85e3 590 if(hand.read()==0)i++;
yuto17320508 14:1a3a673d85e3 591 }
yuto17320508 12:9a5de6adae9c 592 }