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@16:1e91753f0a01, 2019-05-02 (annotated)

Committer:
shimizuta
Date:
Thu May 02 09:24:28 2019 +0000
Revision:
16:1e91753f0a01
Parent:
15:1098bf926b5b
Child:
17:2b3fa9b1a05b
a

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