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@11:a6fadff7cc78, 2019-05-02 (annotated)

Committer:
kageyuta
Date:
Thu May 02 00:51:47 2019 +0000
Revision:
11:a6fadff7cc78
Parent:
10:a335588b9ef0
Child:
12:9a5de6adae9c
k

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