yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

main.cpp@8:43c31d2afb9e, 2019-05-02 (annotated)

Committer:
yuto17320508
Date:
Thu May 02 09:08:26 2019 +0000
Revision:
8:43c31d2afb9e
Parent:
7:c5c60192eb02
Child:
9:ac95473a5d86
l

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:411ab20ce87d 1 #include "mbed.h"
eri 0:411ab20ce87d 2 #include "pin.h"
eri 0:411ab20ce87d 3
eri 0:411ab20ce87d 4
yuto17320508 2:db2bc2ae4d20 5 //#define DEBUG_ON
yuto17320508 2:db2bc2ae4d20 6
yuto17320508 2:db2bc2ae4d20 7 #ifdef DEBUG_ON
yuto17320508 2:db2bc2ae4d20 8 #define DEBUG(...) printf("" __VA_ARGS__);
yuto17320508 2:db2bc2ae4d20 9 #else
yuto17320508 2:db2bc2ae4d20 10 #define DEBUG(...)
yuto17320508 2:db2bc2ae4d20 11 #endif
yuto17320508 2:db2bc2ae4d20 12
yuto17320508 2:db2bc2ae4d20 13 #define Pi 3.14159265359 //円周率π
eri 0:411ab20ce87d 14
yuto17320508 7:c5c60192eb02 15 enum WALKMODE
yuto17320508 7:c5c60192eb02 16 {
yuto17320508 7:c5c60192eb02 17 BACK,
yuto17320508 7:c5c60192eb02 18 STOP,
yuto17320508 7:c5c60192eb02 19 FORWARD,
yuto17320508 7:c5c60192eb02 20 };
yuto17320508 7:c5c60192eb02 21 enum EVENT
yuto17320508 7:c5c60192eb02 22 {
yuto17320508 7:c5c60192eb02 23 NORMAL,
yuto17320508 7:c5c60192eb02 24 GEREGE,
yuto17320508 7:c5c60192eb02 25 GOAL,
yuto17320508 7:c5c60192eb02 26 };
kageyuta 1:34371ffd3dc0 27
yuto17320508 2:db2bc2ae4d20 28 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
yuto17320508 2:db2bc2ae4d20 29
yuto17320508 2:db2bc2ae4d20 30 class PIDcontroller //distanceをvalueに置き換えました
yuto17320508 2:db2bc2ae4d20 31 {
yuto17320508 2:db2bc2ae4d20 32 float Kp_, Ki_, Kd_, tolerance_, time_delta_;
yuto17320508 2:db2bc2ae4d20 33 float pile_, value_old_, target_;
yuto17320508 2:db2bc2ae4d20 34
yuto17320508 7:c5c60192eb02 35 public:
yuto17320508 2:db2bc2ae4d20 36 bool IsConvergence_; //収束したかどうか
yuto17320508 2:db2bc2ae4d20 37 PIDcontroller(float Kp, float Ki, float Kd); //初期設定で係数を入力
yuto17320508 7:c5c60192eb02 38 void setCoefficients(float Kp, float Ki, float Kd)
yuto17320508 7:c5c60192eb02 39 {
yuto17320508 7:c5c60192eb02 40 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 7:c5c60192eb02 41 }; //係数を変更するときに使う
yuto17320508 7:c5c60192eb02 42 void setTimeDelta(float delta)
yuto17320508 7:c5c60192eb02 43 {
yuto17320508 7:c5c60192eb02 44 time_delta_ = delta;
yuto17320508 7:c5c60192eb02 45 };
yuto17320508 2:db2bc2ae4d20 46 void setTarget(float target); //目標位置の設定
yuto17320508 7:c5c60192eb02 47 void setTolerance(float tolerance)
yuto17320508 7:c5c60192eb02 48 {
yuto17320508 7:c5c60192eb02 49 tolerance_ = tolerance;
yuto17320508 7:c5c60192eb02 50 }; //許容誤差の設定
yuto17320508 2:db2bc2ae4d20 51 float calc(float nowVal); //現在位置と目標を比較してPID補正
yuto17320508 7:c5c60192eb02 52 bool knowConvergence()
yuto17320508 7:c5c60192eb02 53 {
yuto17320508 7:c5c60192eb02 54 return IsConvergence_;
yuto17320508 7:c5c60192eb02 55 }; //収束したかどうかを外部に伝える
yuto17320508 2:db2bc2ae4d20 56 };
yuto17320508 2:db2bc2ae4d20 57
yuto17320508 2:db2bc2ae4d20 58 class Motor //PIDコントローラ、エンコーダを含むモータのクラス
yuto17320508 2:db2bc2ae4d20 59 {
yuto17320508 2:db2bc2ae4d20 60 PwmOut *pin_forward_, *pin_back_;
yuto17320508 2:db2bc2ae4d20 61 Ec *ec_; //対応するエンコーダ
yuto17320508 2:db2bc2ae4d20 62 float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
yuto17320508 2:db2bc2ae4d20 63 int resolution_;
yuto17320508 7:c5c60192eb02 64 public:
yuto17320508 2:db2bc2ae4d20 65 Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
yuto17320508 7:c5c60192eb02 66 void setDutyLimit(float limit)
yuto17320508 7:c5c60192eb02 67 {
yuto17320508 7:c5c60192eb02 68 duty_limit_ = limit;
yuto17320508 7:c5c60192eb02 69 };
yuto17320508 2:db2bc2ae4d20 70 float getPosi(); //ポジをエンコーダから取得
yuto17320508 2:db2bc2ae4d20 71 void calcDuty(PIDcontroller *pid); //Duty比を計算
yuto17320508 7:c5c60192eb02 72 void setEncoder(Ec *ec)
yuto17320508 7:c5c60192eb02 73 {
yuto17320508 7:c5c60192eb02 74 ec_ = ec;
yuto17320508 7:c5c60192eb02 75 }; //エンコーダを設定
yuto17320508 7:c5c60192eb02 76 void setResolution(int reso)
yuto17320508 7:c5c60192eb02 77 {
yuto17320508 7:c5c60192eb02 78 resolution_ = reso;
yuto17320508 7:c5c60192eb02 79 };
yuto17320508 2:db2bc2ae4d20 80 void output(); //出力するだけ
yuto17320508 2:db2bc2ae4d20 81 void output(float duty);
yuto17320508 2:db2bc2ae4d20 82 };
yuto17320508 2:db2bc2ae4d20 83
yuto17320508 2:db2bc2ae4d20 84 class OneLeg //足の挙動を制御する
yuto17320508 2:db2bc2ae4d20 85 {
yuto17320508 2:db2bc2ae4d20 86 Motor *motor_;
yuto17320508 2:db2bc2ae4d20 87 float target_pose_;
yuto17320508 2:db2bc2ae4d20 88
yuto17320508 7:c5c60192eb02 89 public:
yuto17320508 2:db2bc2ae4d20 90 PIDcontroller *pid_;
yuto17320508 7:c5c60192eb02 91 OneLeg() {};
yuto17320508 7:c5c60192eb02 92 void setMotor(Motor *motor)
yuto17320508 7:c5c60192eb02 93 {
yuto17320508 7:c5c60192eb02 94 motor_ = motor;
yuto17320508 7:c5c60192eb02 95 };
yuto17320508 7:c5c60192eb02 96 void setPIDcontroller(PIDcontroller *pid)
yuto17320508 7:c5c60192eb02 97 {
yuto17320508 7:c5c60192eb02 98 pid_ = pid;
yuto17320508 7:c5c60192eb02 99 };
yuto17320508 2:db2bc2ae4d20 100 void setTargetPose(float target_pose);
yuto17320508 2:db2bc2ae4d20 101 void actMotor();//モータ出力
yuto17320508 2:db2bc2ae4d20 102 };
yuto17320508 2:db2bc2ae4d20 103
yuto17320508 2:db2bc2ae4d20 104 class Robot
yuto17320508 2:db2bc2ae4d20 105 {
yuto17320508 2:db2bc2ae4d20 106 float ticker_time_, air_wait_time_;
yuto17320508 2:db2bc2ae4d20 107 OneLeg *Leg1_, *Leg2_;
yuto17320508 2:db2bc2ae4d20 108 Timer timer;
yuto17320508 2:db2bc2ae4d20 109
yuto17320508 7:c5c60192eb02 110 public:
yuto17320508 7:c5c60192eb02 111 Robot()
yuto17320508 7:c5c60192eb02 112 {
yuto17320508 7:c5c60192eb02 113 timer.reset();
yuto17320508 7:c5c60192eb02 114 timer.start();
yuto17320508 7:c5c60192eb02 115 };
yuto17320508 2:db2bc2ae4d20 116 void setLeg(OneLeg *Leg1_, OneLeg *Leg2_);
yuto17320508 2:db2bc2ae4d20 117 void setTickerTime(float ticker_time);
yuto17320508 2:db2bc2ae4d20 118 void run();//ここがメインで走る記述
yuto17320508 2:db2bc2ae4d20 119 };
yuto17320508 2:db2bc2ae4d20 120
eri 0:411ab20ce87d 121
eri 0:411ab20ce87d 122
yuto17320508 2:db2bc2ae4d20 123 PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
yuto17320508 2:db2bc2ae4d20 124 {
yuto17320508 2:db2bc2ae4d20 125 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 2:db2bc2ae4d20 126 DEBUG("set Kp:%.3f KI:%.3f Kd:%.3f \n\r", Kp_, Ki_, Kd_);
yuto17320508 2:db2bc2ae4d20 127 IsConvergence_=true;
yuto17320508 2:db2bc2ae4d20 128 }
yuto17320508 2:db2bc2ae4d20 129 void PIDcontroller::setTarget(float target)
yuto17320508 2:db2bc2ae4d20 130 {
yuto17320508 7:c5c60192eb02 131 if (IsConvergence_) { //収束時のみ変更可能
yuto17320508 2:db2bc2ae4d20 132 target_ = target;
yuto17320508 2:db2bc2ae4d20 133 DEBUG("set Target: %.3f\n\r", target_);
yuto17320508 2:db2bc2ae4d20 134 IsConvergence_ = false;
yuto17320508 7:c5c60192eb02 135 } else {
yuto17320508 2:db2bc2ae4d20 136 DEBUG("error: setTarget permission denied!\n");
yuto17320508 2:db2bc2ae4d20 137 }
yuto17320508 2:db2bc2ae4d20 138 }
yuto17320508 2:db2bc2ae4d20 139 float PIDcontroller::calc(float nowVal)
yuto17320508 2:db2bc2ae4d20 140 {
yuto17320508 2:db2bc2ae4d20 141 float out = 0;
yuto17320508 2:db2bc2ae4d20 142 //PID計算ここで行う
yuto17320508 2:db2bc2ae4d20 143 float deviation = target_ - nowVal; //目標との差分
yuto17320508 2:db2bc2ae4d20 144 pile_ += deviation; //積分用に和を取る
yuto17320508 2:db2bc2ae4d20 145 out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
yuto17320508 2:db2bc2ae4d20 146 value_old_ = nowVal; //今のデータを保存
yuto17320508 2:db2bc2ae4d20 147 //
yuto17320508 7:c5c60192eb02 148 if (fabs(deviation) < tolerance_) { //収束した場合
yuto17320508 2:db2bc2ae4d20 149 DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
yuto17320508 2:db2bc2ae4d20 150 out = 0;
yuto17320508 2:db2bc2ae4d20 151 pile_ = 0;
yuto17320508 2:db2bc2ae4d20 152 value_old_ = 0;
yuto17320508 2:db2bc2ae4d20 153 IsConvergence_ = true;
yuto17320508 2:db2bc2ae4d20 154 }
yuto17320508 2:db2bc2ae4d20 155 return out;
yuto17320508 2:db2bc2ae4d20 156 }
eri 0:411ab20ce87d 157
yuto17320508 2:db2bc2ae4d20 158 Motor::Motor(PwmOut *forward, PwmOut *back)
yuto17320508 2:db2bc2ae4d20 159 {
yuto17320508 2:db2bc2ae4d20 160 pin_forward_ = forward;
yuto17320508 2:db2bc2ae4d20 161 pin_back_ = back;
yuto17320508 2:db2bc2ae4d20 162 }
yuto17320508 2:db2bc2ae4d20 163 float Motor::getPosi()
yuto17320508 2:db2bc2ae4d20 164 {
yuto17320508 2:db2bc2ae4d20 165 float posi = 2.0*180*((float)(ec_->getCount)()/(float)resolution_);
yuto17320508 7:c5c60192eb02 166
yuto17320508 2:db2bc2ae4d20 167 //DEBUG("value :%d :%d\n\r", (ec_->getCount)(),resolution_);
yuto17320508 2:db2bc2ae4d20 168 DEBUG("posi is %.4f\n\r",posi);
yuto17320508 2:db2bc2ae4d20 169 return posi;
yuto17320508 2:db2bc2ae4d20 170 }
yuto17320508 2:db2bc2ae4d20 171 void Motor::calcDuty(PIDcontroller *pid)
yuto17320508 7:c5c60192eb02 172 {
yuto17320508 2:db2bc2ae4d20 173 duty_ = pid->calc(getPosi());
yuto17320508 2:db2bc2ae4d20 174 DEBUG("duty is %.4f\n\r",duty_);
yuto17320508 2:db2bc2ae4d20 175 }
yuto17320508 2:db2bc2ae4d20 176 void Motor::output()
yuto17320508 2:db2bc2ae4d20 177 {
yuto17320508 2:db2bc2ae4d20 178 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 2:db2bc2ae4d20 179 //加速度が一定値を超えたら変更加える
yuto17320508 7:c5c60192eb02 180 if (duty_ > 0) {
yuto17320508 4:db1640bd0e89 181 //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
yuto17320508 2:db2bc2ae4d20 182 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 2:db2bc2ae4d20 183 pin_forward_->write(output_duty);
yuto17320508 2:db2bc2ae4d20 184 pin_back_->write(0);
yuto17320508 2:db2bc2ae4d20 185 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 7:c5c60192eb02 186 } else {
yuto17320508 4:db1640bd0e89 187 //if (pre_duty_ - duty_ > accel_max)
yuto17320508 4:db1640bd0e89 188 // duty_ = pre_duty_ - accel_max;
yuto17320508 2:db2bc2ae4d20 189 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 2:db2bc2ae4d20 190 pin_forward_->write(0);
yuto17320508 2:db2bc2ae4d20 191 pin_back_->write(output_duty);
yuto17320508 2:db2bc2ae4d20 192 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 2:db2bc2ae4d20 193 }
yuto17320508 2:db2bc2ae4d20 194 pre_duty_ = duty_;
yuto17320508 2:db2bc2ae4d20 195 }
yuto17320508 2:db2bc2ae4d20 196 void Motor::output(float duty)
yuto17320508 2:db2bc2ae4d20 197 {
yuto17320508 2:db2bc2ae4d20 198 duty_ = duty;
yuto17320508 2:db2bc2ae4d20 199 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 2:db2bc2ae4d20 200 //加速度が一定値を超えたら変更加える
yuto17320508 7:c5c60192eb02 201 if (duty_ > 0) {
yuto17320508 2:db2bc2ae4d20 202 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 2:db2bc2ae4d20 203 pin_forward_->write(output_duty);
yuto17320508 2:db2bc2ae4d20 204 pin_back_->write(0);
yuto17320508 2:db2bc2ae4d20 205 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 7:c5c60192eb02 206 } else {
yuto17320508 2:db2bc2ae4d20 207 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 2:db2bc2ae4d20 208 pin_forward_->write(0);
yuto17320508 2:db2bc2ae4d20 209 pin_back_->write(output_duty);
yuto17320508 2:db2bc2ae4d20 210 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 2:db2bc2ae4d20 211 }
yuto17320508 2:db2bc2ae4d20 212 }
kageyuta 1:34371ffd3dc0 213
yuto17320508 2:db2bc2ae4d20 214 void OneLeg::setTargetPose(float target_pose)
yuto17320508 2:db2bc2ae4d20 215 {
yuto17320508 2:db2bc2ae4d20 216 target_pose_ = target_pose;
yuto17320508 2:db2bc2ae4d20 217 //PIDにtargetを送る
yuto17320508 2:db2bc2ae4d20 218 pid_->setTarget(target_pose_);
yuto17320508 2:db2bc2ae4d20 219 }
yuto17320508 2:db2bc2ae4d20 220 void OneLeg::actMotor()
yuto17320508 2:db2bc2ae4d20 221 {
yuto17320508 2:db2bc2ae4d20 222 motor_->calcDuty(pid_);
yuto17320508 2:db2bc2ae4d20 223 motor_->output();
yuto17320508 2:db2bc2ae4d20 224 }
yuto17320508 2:db2bc2ae4d20 225
yuto17320508 2:db2bc2ae4d20 226
yuto17320508 2:db2bc2ae4d20 227
yuto17320508 2:db2bc2ae4d20 228 void Robot::setLeg(OneLeg *Leg1, OneLeg *Leg2)
yuto17320508 2:db2bc2ae4d20 229 {
yuto17320508 2:db2bc2ae4d20 230 Leg1_ = Leg1;
yuto17320508 2:db2bc2ae4d20 231 Leg2_ = Leg2;
yuto17320508 2:db2bc2ae4d20 232 }
yuto17320508 2:db2bc2ae4d20 233 void Robot::setTickerTime(float ticker_time)
yuto17320508 2:db2bc2ae4d20 234 {
yuto17320508 2:db2bc2ae4d20 235 ticker_time_ = ticker_time;
yuto17320508 2:db2bc2ae4d20 236 Leg1_->pid_->setTimeDelta(ticker_time_);
yuto17320508 2:db2bc2ae4d20 237 Leg2_->pid_->setTimeDelta(ticker_time_);
yuto17320508 2:db2bc2ae4d20 238 }
yuto17320508 2:db2bc2ae4d20 239 void Robot::run()
yuto17320508 2:db2bc2ae4d20 240 {
yuto17320508 7:c5c60192eb02 241 while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) { //片方が収束していない時*/
yuto17320508 2:db2bc2ae4d20 242 //ticker_time毎にモータに出力する
yuto17320508 2:db2bc2ae4d20 243 float time_s = timer.read();
yuto17320508 2:db2bc2ae4d20 244 Leg1_->actMotor();
yuto17320508 2:db2bc2ae4d20 245 Leg2_->actMotor();
yuto17320508 2:db2bc2ae4d20 246 float rest_time_s = ticker_time_ - (timer.read() - time_s);
yuto17320508 2:db2bc2ae4d20 247 //ticker_timeまで待機
yuto17320508 7:c5c60192eb02 248 if (rest_time_s > 0) {
yuto17320508 2:db2bc2ae4d20 249 wait(rest_time_s);
yuto17320508 2:db2bc2ae4d20 250 DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
yuto17320508 2:db2bc2ae4d20 251 }
yuto17320508 7:c5c60192eb02 252
yuto17320508 2:db2bc2ae4d20 253 else //時間が足りない場合警告
yuto17320508 2:db2bc2ae4d20 254 printf("error: restTime not enough\n\r");
yuto17320508 2:db2bc2ae4d20 255 DEBUG("loop end\n\r")
yuto17320508 2:db2bc2ae4d20 256 }
yuto17320508 2:db2bc2ae4d20 257
yuto17320508 2:db2bc2ae4d20 258 }
eri 0:411ab20ce87d 259
eri 0:411ab20ce87d 260
eri 0:411ab20ce87d 261
eri 0:411ab20ce87d 262
eri 0:411ab20ce87d 263 ////////////変数
yuto17320508 8:43c31d2afb9e 264 int hand_mode=NORMAL;
yuto17320508 2:db2bc2ae4d20 265
yuto17320508 2:db2bc2ae4d20 266 ////////////関数
yuto17320508 2:db2bc2ae4d20 267 void setup();
yuto17320508 7:c5c60192eb02 268 void forward();
yuto17320508 7:c5c60192eb02 269 void stop();
yuto17320508 7:c5c60192eb02 270 void back();
yuto17320508 7:c5c60192eb02 271 void can_receive(int &mode, float &duty);
yuto17320508 2:db2bc2ae4d20 272 void reset();
eri 0:411ab20ce87d 273
yuto17320508 7:c5c60192eb02 274
yuto17320508 2:db2bc2ae4d20 275 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
yuto17320508 2:db2bc2ae4d20 276 //しかし変更を多々行うためポインタ渡しにしてある
yuto17320508 2:db2bc2ae4d20 277 //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
yuto17320508 2:db2bc2ae4d20 278 PIDcontroller pid_ro(0.01, 0.000, 0.000);
yuto17320508 2:db2bc2ae4d20 279 PIDcontroller pid_ri(0.01, 0.000, 0.000); //Kp.Ki,Kd
yuto17320508 2:db2bc2ae4d20 280 Motor motor_ro(&motor_ro_f, &motor_ro_b),
yuto17320508 7:c5c60192eb02 281 motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入
yuto17320508 2:db2bc2ae4d20 282 OneLeg leg_ro, leg_ri;
yuto17320508 2:db2bc2ae4d20 283 Robot robot;
eri 0:411ab20ce87d 284
yuto17320508 7:c5c60192eb02 285 int step_num_r = 0;
eri 0:411ab20ce87d 286 //////////////////////////////////////////////
kageyuta 1:34371ffd3dc0 287 int main()
kageyuta 1:34371ffd3dc0 288 {
yuto17320508 2:db2bc2ae4d20 289 printf("standby ok\n\r");
eri 0:411ab20ce87d 290 setup();
yuto17320508 7:c5c60192eb02 291
yuto17320508 7:c5c60192eb02 292
yuto17320508 2:db2bc2ae4d20 293 pid_ro.setTolerance(10);
yuto17320508 2:db2bc2ae4d20 294 pid_ri.setTolerance(10);
yuto17320508 7:c5c60192eb02 295
yuto17320508 2:db2bc2ae4d20 296 motor_ro.setEncoder(&ec_ro);
yuto17320508 2:db2bc2ae4d20 297 motor_ro.setResolution(1000);
yuto17320508 2:db2bc2ae4d20 298 motor_ri.setEncoder(&ec_ri);
yuto17320508 2:db2bc2ae4d20 299 motor_ri.setResolution(1000);
yuto17320508 7:c5c60192eb02 300
yuto17320508 2:db2bc2ae4d20 301 leg_ro.setMotor(&motor_ro);
yuto17320508 2:db2bc2ae4d20 302 leg_ro.setPIDcontroller(&pid_ro);
yuto17320508 2:db2bc2ae4d20 303 leg_ri.setMotor(&motor_ri);
yuto17320508 2:db2bc2ae4d20 304 leg_ri.setPIDcontroller(&pid_ri);
yuto17320508 7:c5c60192eb02 305
yuto17320508 2:db2bc2ae4d20 306 robot.setLeg(&leg_ro, &leg_ri);
yuto17320508 2:db2bc2ae4d20 307 robot.setTickerTime(0.01); //モータ出力間隔 0.01
yuto17320508 7:c5c60192eb02 308
yuto17320508 6:2bc2950f32d8 309 motor_ro.setDutyLimit(0.5);
yuto17320508 6:2bc2950f32d8 310 motor_ri.setDutyLimit(0.5);
yuto17320508 7:c5c60192eb02 311
kageyuta 1:34371ffd3dc0 312 reset();
yuto17320508 2:db2bc2ae4d20 313 bus_out = 1;
yuto17320508 2:db2bc2ae4d20 314 printf("start\n\r");
yuto17320508 7:c5c60192eb02 315 int mode = -1;
kageyuta 3:29999b02e940 316
yuto17320508 7:c5c60192eb02 317 air_hand = 1;
kageyuta 3:29999b02e940 318
yuto17320508 2:db2bc2ae4d20 319 while(1)
yuto17320508 2:db2bc2ae4d20 320 {
yuto17320508 7:c5c60192eb02 321 float duty = 0.0;
yuto17320508 7:c5c60192eb02 322 can_receive(mode,duty);
yuto17320508 7:c5c60192eb02 323 printf("mode:%d duty:%.3f\n\r", mode, duty);
yuto17320508 7:c5c60192eb02 324 motor_ro.setDutyLimit(duty);
yuto17320508 7:c5c60192eb02 325 motor_ri.setDutyLimit(duty);
yuto17320508 7:c5c60192eb02 326 if(hand_mode == GEREGE)
yuto17320508 2:db2bc2ae4d20 327 {
yuto17320508 7:c5c60192eb02 328 air_hand = 0;
yuto17320508 2:db2bc2ae4d20 329 }
yuto17320508 7:c5c60192eb02 330 else if(hand_mode == GOAL)
yuto17320508 7:c5c60192eb02 331 {
yuto17320508 8:43c31d2afb9e 332 for(int i=0;i<10;++i)
yuto17320508 7:c5c60192eb02 333 servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500));
yuto17320508 7:c5c60192eb02 334 }
yuto17320508 7:c5c60192eb02 335 bus_out = 0;
yuto17320508 7:c5c60192eb02 336 //printf("target is %d\n\r",target_ro);
yuto17320508 7:c5c60192eb02 337 if(mode == FORWARD) forward();
yuto17320508 7:c5c60192eb02 338 else if(mode == STOP) stop();
yuto17320508 7:c5c60192eb02 339 else if(mode == BACK) back();
yuto17320508 2:db2bc2ae4d20 340 bus_out = 1;
eri 0:411ab20ce87d 341 }
yuto17320508 7:c5c60192eb02 342
eri 0:411ab20ce87d 343 }
eri 0:411ab20ce87d 344
yuto17320508 7:c5c60192eb02 345 void forward()
yuto17320508 4:db1640bd0e89 346 {
yuto17320508 7:c5c60192eb02 347 leg_ro.setTargetPose(360+step_num_r*180);
yuto17320508 7:c5c60192eb02 348 leg_ri.setTargetPose(180+step_num_r*180);
yuto17320508 7:c5c60192eb02 349 robot.run();
yuto17320508 7:c5c60192eb02 350 motor_ro_f.write(0);
yuto17320508 7:c5c60192eb02 351 motor_ro_b.write(0);
yuto17320508 7:c5c60192eb02 352 motor_ri_f.write(0);
yuto17320508 7:c5c60192eb02 353 motor_ri_b.write(0);
yuto17320508 7:c5c60192eb02 354 step_num_r++;
yuto17320508 7:c5c60192eb02 355 }
yuto17320508 7:c5c60192eb02 356
yuto17320508 7:c5c60192eb02 357 void stop()
yuto17320508 7:c5c60192eb02 358 {
yuto17320508 7:c5c60192eb02 359 leg_ro.setTargetPose(360+(step_num_r-1)*180);
yuto17320508 7:c5c60192eb02 360 leg_ri.setTargetPose(180+(step_num_r-1)*180);
yuto17320508 4:db1640bd0e89 361 robot.run();
yuto17320508 4:db1640bd0e89 362 motor_ro_f.write(0);
yuto17320508 4:db1640bd0e89 363 motor_ro_b.write(0);
yuto17320508 4:db1640bd0e89 364 motor_ri_f.write(0);
yuto17320508 4:db1640bd0e89 365 motor_ri_b.write(0);
yuto17320508 4:db1640bd0e89 366 }
yuto17320508 7:c5c60192eb02 367
yuto17320508 7:c5c60192eb02 368 void back()
yuto17320508 4:db1640bd0e89 369 {
yuto17320508 7:c5c60192eb02 370 leg_ro.setTargetPose(360+(step_num_r-2)*180);
yuto17320508 7:c5c60192eb02 371 leg_ri.setTargetPose(180+(step_num_r-2)*180);
yuto17320508 4:db1640bd0e89 372 robot.run();
yuto17320508 4:db1640bd0e89 373 motor_ro_f.write(0);
yuto17320508 4:db1640bd0e89 374 motor_ro_b.write(0);
yuto17320508 4:db1640bd0e89 375 motor_ri_f.write(0);
yuto17320508 4:db1640bd0e89 376 motor_ri_b.write(0);
yuto17320508 7:c5c60192eb02 377 step_num_r--;
yuto17320508 4:db1640bd0e89 378 }
yuto17320508 4:db1640bd0e89 379
yuto17320508 4:db1640bd0e89 380
eri 0:411ab20ce87d 381 //////////////////////////////////////////////
kageyuta 1:34371ffd3dc0 382 void setup()
kageyuta 1:34371ffd3dc0 383 {
eri 0:411ab20ce87d 384 can.frequency(1000000);
eri 0:411ab20ce87d 385 motor_ro_f.period_us(100);
eri 0:411ab20ce87d 386 motor_ro_b.period_us(100);
eri 0:411ab20ce87d 387 motor_ri_f.period_us(100);
eri 0:411ab20ce87d 388 motor_ri_b.period_us(100);
kageyuta 1:34371ffd3dc0 389
kageyuta 1:34371ffd3dc0 390 switch_ro.mode(PullUp);
kageyuta 1:34371ffd3dc0 391 switch_ri.mode(PullUp);
kageyuta 1:34371ffd3dc0 392
eri 0:411ab20ce87d 393 servo.init();
yuto17320508 7:c5c60192eb02 394 wait(0.01);
yuto17320508 7:c5c60192eb02 395 for(int i=0;i<5;++i)
yuto17320508 7:c5c60192eb02 396 servo.set_degree(0,0);
yuto17320508 7:c5c60192eb02 397 //(7200 - 3500) * 270.0/(11500 - 3500)
eri 0:411ab20ce87d 398 }
eri 0:411ab20ce87d 399
yuto17320508 4:db1640bd0e89 400
yuto17320508 4:db1640bd0e89 401
kageyuta 1:34371ffd3dc0 402 void reset()
kageyuta 1:34371ffd3dc0 403 {
kageyuta 1:34371ffd3dc0 404 while(switch_ro.read()) {
kageyuta 3:29999b02e940 405 motor_ro.output(0.3);
kageyuta 1:34371ffd3dc0 406 }
yuto17320508 7:c5c60192eb02 407 ec_ro.reset();
yuto17320508 7:c5c60192eb02 408 motor_ro.output(0.0);
yuto17320508 7:c5c60192eb02 409 printf("ro OK\n\r");
kageyuta 3:29999b02e940 410 while(switch_ri.read()) {
kageyuta 3:29999b02e940 411 motor_ri.output(0.3);
kageyuta 3:29999b02e940 412 }
yuto17320508 7:c5c60192eb02 413
yuto17320508 7:c5c60192eb02 414 ec_ri.reset();
yuto17320508 7:c5c60192eb02 415 motor_ri.output(0.0);
yuto17320508 7:c5c60192eb02 416 printf("ri OK\n\r");
eri 0:411ab20ce87d 417 }
eri 0:411ab20ce87d 418
eri 0:411ab20ce87d 419
yuto17320508 4:db1640bd0e89 420
yuto17320508 4:db1640bd0e89 421
eri 0:411ab20ce87d 422 //////////////////////////////////////////can
yuto17320508 7:c5c60192eb02 423 void can_receive(int &mode, float &duty)
kageyuta 1:34371ffd3dc0 424 {
eri 0:411ab20ce87d 425 CANMessage msg;
yuto17320508 7:c5c60192eb02 426 while(1) {
yuto17320508 7:c5c60192eb02 427 //printf("roop\r\n");
eri 0:411ab20ce87d 428 if(can.read(msg)) {
eri 0:411ab20ce87d 429 if(msg.id==0) {
yuto17320508 7:c5c60192eb02 430 mode= msg.data[0]*0.1;
yuto17320508 7:c5c60192eb02 431 duty = (msg.data[0]%10)*0.1;
yuto17320508 7:c5c60192eb02 432 hand_mode= msg.data[1];
eri 0:411ab20ce87d 433 break;
eri 0:411ab20ce87d 434 }
kageyuta 1:34371ffd3dc0 435 led2=1;
eri 0:411ab20ce87d 436 } else led2=0;
eri 0:411ab20ce87d 437 }
yuto17320508 7:c5c60192eb02 438 }