yuto kawamura / Mbed 2 deprecated MR2_n_1_class

l

Dependencies:   mbed

main.cpp@6:75cfa1a66382, 2019-04-27 (annotated)

Committer:
yuto17320508
Date:
Sat Apr 27 11:22:00 2019 +0000
Revision:
6:75cfa1a66382
Parent:
5:1fa5aa097af5 
l

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:111abd91b0cb 1 #include "mbed.h"
eri 0:111abd91b0cb 2 #include "EC.h"
eri 0:111abd91b0cb 3 #include "KondoServo.h"
eri 0:111abd91b0cb 4 #include "hcsr04.h"
eri 3:df0c9883e403 5 #include "pin.h"
eri 3:df0c9883e403 6 #include "microinfinity.h"
eri 0:111abd91b0cb 7
yuto17320508 6:75cfa1a66382 8 //#define DEBUG_ON
eri 3:df0c9883e403 9
yuto17320508 5:1fa5aa097af5 10 #ifdef DEBUG_ON
yuto17320508 5:1fa5aa097af5 11 #define DEBUG(...) printf("" __VA_ARGS__);
yuto17320508 5:1fa5aa097af5 12 #else
yuto17320508 5:1fa5aa097af5 13 #define DEBUG(...)
yuto17320508 5:1fa5aa097af5 14 #endif
yuto17320508 5:1fa5aa097af5 15
yuto17320508 5:1fa5aa097af5 16
yuto17320508 5:1fa5aa097af5 17 int ecgear = 25; //ここはいちいち設定したくないからグローバル
yuto17320508 5:1fa5aa097af5 18 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
yuto17320508 5:1fa5aa097af5 19
yuto17320508 5:1fa5aa097af5 20 #define Pi 3.14159265359 //円周率π
yuto17320508 5:1fa5aa097af5 21
yuto17320508 5:1fa5aa097af5 22 class PIDcontroller //distanceをvalueに置き換えました
yuto17320508 5:1fa5aa097af5 23 {
yuto17320508 5:1fa5aa097af5 24 float Kp_, Ki_, Kd_, tolerance_, time_delta_;
yuto17320508 5:1fa5aa097af5 25 float pile_, value_old_, target_;
yuto17320508 5:1fa5aa097af5 26
yuto17320508 5:1fa5aa097af5 27 public:
yuto17320508 5:1fa5aa097af5 28 bool IsConvergence_; //収束したかどうか
yuto17320508 5:1fa5aa097af5 29 PIDcontroller(float Kp, float Ki, float Kd); //初期設定で係数を入力
yuto17320508 5:1fa5aa097af5 30 void setCoefficients(float Kp, float Ki, float Kd); //係数を変更するときに使う
yuto17320508 5:1fa5aa097af5 31 void setTimeDelta(float delta);
yuto17320508 5:1fa5aa097af5 32 void setTarget(float target); //目標位置の設定
yuto17320508 5:1fa5aa097af5 33 void setTolerance(float tolerance); //許容誤差の設定
yuto17320508 5:1fa5aa097af5 34 float calc(float nowVal); //現在位置と目標を比較してPID補正
yuto17320508 5:1fa5aa097af5 35 bool knowConvergence(); //収束したかどうかを外部に伝える
yuto17320508 5:1fa5aa097af5 36 };
yuto17320508 5:1fa5aa097af5 37
yuto17320508 5:1fa5aa097af5 38 class Motor //PIDコントローラ、エンコーダを含むモータのクラス
yuto17320508 5:1fa5aa097af5 39 {
yuto17320508 5:1fa5aa097af5 40 PwmOut *pin_forward_, *pin_back_;
yuto17320508 5:1fa5aa097af5 41 Ec *ec_; //対応するエンコーダ
yuto17320508 5:1fa5aa097af5 42 float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
yuto17320508 5:1fa5aa097af5 43 public:
yuto17320508 5:1fa5aa097af5 44 Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
yuto17320508 5:1fa5aa097af5 45 void setDutyLimit(float limit);
yuto17320508 5:1fa5aa097af5 46 float getPosi(); //ポジをエンコーダから取得
yuto17320508 5:1fa5aa097af5 47 void calcDuty(PIDcontroller *pid); //Duty比を計算
yuto17320508 5:1fa5aa097af5 48 void setEncoder(Ec *ec); //エンコーダを設定
yuto17320508 5:1fa5aa097af5 49 void output(); //出力するだけ
yuto17320508 5:1fa5aa097af5 50 };
eri 0:111abd91b0cb 51
yuto17320508 5:1fa5aa097af5 52 class AirCylinder
yuto17320508 5:1fa5aa097af5 53 {
yuto17320508 5:1fa5aa097af5 54 DigitalOut *air_;
yuto17320508 5:1fa5aa097af5 55 bool IsOpenState_;
yuto17320508 5:1fa5aa097af5 56
yuto17320508 5:1fa5aa097af5 57 public:
yuto17320508 5:1fa5aa097af5 58 AirCylinder(DigitalOut *air); //ピンをポインタ渡し
yuto17320508 5:1fa5aa097af5 59 void open(); //足上げ
yuto17320508 5:1fa5aa097af5 60 void close(); //足下げ
yuto17320508 5:1fa5aa097af5 61 void changeState(); //上がってるとき下げる　その逆も
yuto17320508 5:1fa5aa097af5 62 };
yuto17320508 5:1fa5aa097af5 63
yuto17320508 5:1fa5aa097af5 64 class OneLeg //足の挙動を制御する
yuto17320508 5:1fa5aa097af5 65 {
yuto17320508 5:1fa5aa097af5 66 Motor *motor_;
yuto17320508 5:1fa5aa097af5 67 AirCylinder *air1_, *air2_;
yuto17320508 5:1fa5aa097af5 68 float target_pose_;
eri 0:111abd91b0cb 69
yuto17320508 5:1fa5aa097af5 70 public:
yuto17320508 5:1fa5aa097af5 71 PIDcontroller *pid_;
yuto17320508 5:1fa5aa097af5 72 OneLeg(){};
yuto17320508 5:1fa5aa097af5 73 void setMotor(Motor *motor);
yuto17320508 5:1fa5aa097af5 74 void setPIDcontroller(PIDcontroller *pid);
yuto17320508 5:1fa5aa097af5 75 void setAir(AirCylinder *air1, AirCylinder *air2);
yuto17320508 5:1fa5aa097af5 76 void setTargetPose(float target_pose);
yuto17320508 5:1fa5aa097af5 77 void actMotor();//モータ出力
yuto17320508 5:1fa5aa097af5 78 void actCylinder();//シリンダ出力
yuto17320508 5:1fa5aa097af5 79 };
yuto17320508 5:1fa5aa097af5 80
yuto17320508 5:1fa5aa097af5 81
yuto17320508 5:1fa5aa097af5 82 class Robot
yuto17320508 5:1fa5aa097af5 83 {
yuto17320508 5:1fa5aa097af5 84 float ticker_time_, air_wait_time_;
yuto17320508 5:1fa5aa097af5 85 OneLeg *rightLeg_, *leftLeg_;
yuto17320508 5:1fa5aa097af5 86 Timer timer;
yuto17320508 5:1fa5aa097af5 87
yuto17320508 5:1fa5aa097af5 88 public:
yuto17320508 6:75cfa1a66382 89 Robot(){timer.reset(); timer.start();};
yuto17320508 5:1fa5aa097af5 90 void setLeg(OneLeg *rightLeg, OneLeg *leftLeg);
yuto17320508 5:1fa5aa097af5 91 void setTickerTime(float ticker_time);
yuto17320508 5:1fa5aa097af5 92 void setAirWaitTime(float air_wait_time);
yuto17320508 5:1fa5aa097af5 93 void run();//ここがメインで走る記述
yuto17320508 5:1fa5aa097af5 94 };
yuto17320508 5:1fa5aa097af5 95
eri 0:111abd91b0cb 96
eri 0:111abd91b0cb 97
eri 0:111abd91b0cb 98
yuto17320508 5:1fa5aa097af5 99 PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
yuto17320508 5:1fa5aa097af5 100 {
yuto17320508 5:1fa5aa097af5 101 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 5:1fa5aa097af5 102 DEBUG("set Kp:%.3f KI:%.3f Kd:%.3f \n\r", Kp_, Ki_, Kd_);
yuto17320508 5:1fa5aa097af5 103 IsConvergence_=true;
yuto17320508 5:1fa5aa097af5 104 }
yuto17320508 5:1fa5aa097af5 105 void PIDcontroller::setCoefficients(float Kp, float Ki, float Kd)
yuto17320508 5:1fa5aa097af5 106 {
yuto17320508 5:1fa5aa097af5 107 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 5:1fa5aa097af5 108 }
yuto17320508 5:1fa5aa097af5 109 void PIDcontroller::setTimeDelta(float delta)
yuto17320508 5:1fa5aa097af5 110 {
yuto17320508 5:1fa5aa097af5 111 time_delta_ = delta;
yuto17320508 5:1fa5aa097af5 112 DEBUG("set TimeDelta: %.3f\n\r", time_delta_);
yuto17320508 5:1fa5aa097af5 113 }
yuto17320508 5:1fa5aa097af5 114 void PIDcontroller::setTarget(float target)
yuto17320508 5:1fa5aa097af5 115 {
yuto17320508 5:1fa5aa097af5 116 if (IsConvergence_) //収束時のみ変更可能
yuto17320508 5:1fa5aa097af5 117 {
yuto17320508 5:1fa5aa097af5 118 target_ = target;
yuto17320508 5:1fa5aa097af5 119 DEBUG("set Target: %.3f\n\r", target_);
yuto17320508 5:1fa5aa097af5 120 IsConvergence_ = false;
yuto17320508 5:1fa5aa097af5 121 }
yuto17320508 5:1fa5aa097af5 122 else
yuto17320508 5:1fa5aa097af5 123 {
yuto17320508 5:1fa5aa097af5 124 DEBUG("error: setTarget permission denied!\n");
yuto17320508 5:1fa5aa097af5 125 }
yuto17320508 5:1fa5aa097af5 126 }
yuto17320508 5:1fa5aa097af5 127 void PIDcontroller::setTolerance(float tolerance)
yuto17320508 5:1fa5aa097af5 128 {
yuto17320508 5:1fa5aa097af5 129 tolerance_ = tolerance;
yuto17320508 5:1fa5aa097af5 130 DEBUG("set Tolerance: %.3f\n\r", tolerance_);
yuto17320508 5:1fa5aa097af5 131 }
yuto17320508 5:1fa5aa097af5 132 float PIDcontroller::calc(float nowVal)
yuto17320508 5:1fa5aa097af5 133 {
yuto17320508 5:1fa5aa097af5 134 float out = 0;
yuto17320508 5:1fa5aa097af5 135 //PID計算ここで行う
yuto17320508 5:1fa5aa097af5 136 float deviation = target_ - nowVal; //目標との差分
yuto17320508 5:1fa5aa097af5 137 pile_ += deviation; //積分用に和を取る
yuto17320508 5:1fa5aa097af5 138 out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
yuto17320508 5:1fa5aa097af5 139 value_old_ = nowVal; //今のデータを保存
yuto17320508 5:1fa5aa097af5 140 //
yuto17320508 5:1fa5aa097af5 141 if (fabs(deviation) < tolerance_) //収束した場合
yuto17320508 5:1fa5aa097af5 142 {
yuto17320508 5:1fa5aa097af5 143 DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
yuto17320508 5:1fa5aa097af5 144 out = 0;
yuto17320508 5:1fa5aa097af5 145 pile_ = 0;
yuto17320508 5:1fa5aa097af5 146 value_old_ = 0;
yuto17320508 5:1fa5aa097af5 147 IsConvergence_ = true;
yuto17320508 5:1fa5aa097af5 148 }
yuto17320508 5:1fa5aa097af5 149 return out;
yuto17320508 5:1fa5aa097af5 150 }
yuto17320508 5:1fa5aa097af5 151 bool PIDcontroller::knowConvergence()
yuto17320508 5:1fa5aa097af5 152 {
yuto17320508 5:1fa5aa097af5 153 return IsConvergence_;
yuto17320508 5:1fa5aa097af5 154 }
eri 0:111abd91b0cb 155
yuto17320508 5:1fa5aa097af5 156 Motor::Motor(PwmOut *forward, PwmOut *back)
yuto17320508 5:1fa5aa097af5 157 {
yuto17320508 5:1fa5aa097af5 158 pin_forward_ = forward;
yuto17320508 5:1fa5aa097af5 159 pin_back_ = back;
yuto17320508 5:1fa5aa097af5 160 }
yuto17320508 5:1fa5aa097af5 161 float Motor::getPosi()
yuto17320508 5:1fa5aa097af5 162 {
yuto17320508 5:1fa5aa097af5 163 float posi = 1.0 * ecgear * Pi * (ec_->getCount)() /resolution;
yuto17320508 5:1fa5aa097af5 164 DEBUG("posi is %.4f\n\r",posi);
yuto17320508 5:1fa5aa097af5 165 return posi;
yuto17320508 5:1fa5aa097af5 166 }
yuto17320508 5:1fa5aa097af5 167 void Motor::setDutyLimit(float limit)
yuto17320508 5:1fa5aa097af5 168 {
yuto17320508 5:1fa5aa097af5 169 duty_limit_ = limit;
yuto17320508 5:1fa5aa097af5 170 DEBUG("set DutyLimit: %.3f\n\r", duty_limit_);
yuto17320508 5:1fa5aa097af5 171 }
yuto17320508 5:1fa5aa097af5 172 void Motor::calcDuty(PIDcontroller *pid)
yuto17320508 5:1fa5aa097af5 173 {
yuto17320508 5:1fa5aa097af5 174 duty_ = pid->calc(getPosi());
yuto17320508 5:1fa5aa097af5 175 DEBUG("duty is %.4f\n\r",duty_);
yuto17320508 5:1fa5aa097af5 176 }
yuto17320508 5:1fa5aa097af5 177 void Motor::setEncoder(Ec *ec)
yuto17320508 5:1fa5aa097af5 178 {
yuto17320508 5:1fa5aa097af5 179 ec_ = ec;
yuto17320508 5:1fa5aa097af5 180 }
yuto17320508 5:1fa5aa097af5 181 void Motor::output()
yuto17320508 5:1fa5aa097af5 182 {
yuto17320508 5:1fa5aa097af5 183 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 5:1fa5aa097af5 184 //加速度が一定値を超えたら変更加える
yuto17320508 5:1fa5aa097af5 185 if (duty_ > 0)
yuto17320508 5:1fa5aa097af5 186 {
yuto17320508 5:1fa5aa097af5 187 if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
yuto17320508 6:75cfa1a66382 188 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 6:75cfa1a66382 189 pin_forward_->write(output_duty);
yuto17320508 6:75cfa1a66382 190 pin_back_->write(0);
yuto17320508 5:1fa5aa097af5 191 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 5:1fa5aa097af5 192 }
yuto17320508 5:1fa5aa097af5 193 else
yuto17320508 5:1fa5aa097af5 194 {
yuto17320508 5:1fa5aa097af5 195 if (pre_duty_ - duty_ > accel_max)
yuto17320508 5:1fa5aa097af5 196 duty_ = pre_duty_ - accel_max;
yuto17320508 6:75cfa1a66382 197 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 6:75cfa1a66382 198 pin_forward_->write(0);
yuto17320508 6:75cfa1a66382 199 pin_back_->write(output_duty);
yuto17320508 5:1fa5aa097af5 200 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 5:1fa5aa097af5 201 }
yuto17320508 5:1fa5aa097af5 202 pre_duty_ = duty_;
yuto17320508 5:1fa5aa097af5 203 }
yuto17320508 5:1fa5aa097af5 204
yuto17320508 5:1fa5aa097af5 205 AirCylinder::AirCylinder(DigitalOut *air)
yuto17320508 5:1fa5aa097af5 206 {
yuto17320508 5:1fa5aa097af5 207 air_ = air;
yuto17320508 5:1fa5aa097af5 208 }
yuto17320508 5:1fa5aa097af5 209 void AirCylinder::open()
yuto17320508 5:1fa5aa097af5 210 {
yuto17320508 5:1fa5aa097af5 211 *air_ = 1;
yuto17320508 5:1fa5aa097af5 212 IsOpenState_ = true;
yuto17320508 5:1fa5aa097af5 213 }
yuto17320508 5:1fa5aa097af5 214 void AirCylinder::close()
yuto17320508 5:1fa5aa097af5 215 {
yuto17320508 5:1fa5aa097af5 216 *air_ = 0;
yuto17320508 5:1fa5aa097af5 217 IsOpenState_ = false;
yuto17320508 5:1fa5aa097af5 218 }
yuto17320508 5:1fa5aa097af5 219 void AirCylinder::changeState()
yuto17320508 5:1fa5aa097af5 220 {
yuto17320508 5:1fa5aa097af5 221 if (IsOpenState_)
yuto17320508 5:1fa5aa097af5 222 close();
yuto17320508 5:1fa5aa097af5 223 else
yuto17320508 5:1fa5aa097af5 224 open();
yuto17320508 5:1fa5aa097af5 225 }
yuto17320508 5:1fa5aa097af5 226
yuto17320508 5:1fa5aa097af5 227 void OneLeg::setMotor(Motor *motor)
yuto17320508 5:1fa5aa097af5 228 {
yuto17320508 5:1fa5aa097af5 229 motor_ = motor;
yuto17320508 5:1fa5aa097af5 230 }
yuto17320508 5:1fa5aa097af5 231 void OneLeg::setPIDcontroller(PIDcontroller *pid)
yuto17320508 5:1fa5aa097af5 232 {
yuto17320508 5:1fa5aa097af5 233 pid_ = pid;
yuto17320508 5:1fa5aa097af5 234 }
yuto17320508 5:1fa5aa097af5 235 void OneLeg::setAir(AirCylinder *air1, AirCylinder *air2)
yuto17320508 5:1fa5aa097af5 236 {
yuto17320508 5:1fa5aa097af5 237 air1_ = air1, air2_ = air2;
yuto17320508 5:1fa5aa097af5 238 }
yuto17320508 5:1fa5aa097af5 239 void OneLeg::setTargetPose(float target_pose)
yuto17320508 5:1fa5aa097af5 240 {
yuto17320508 5:1fa5aa097af5 241 target_pose_ = target_pose;
yuto17320508 5:1fa5aa097af5 242 //PIDにtargetを送る
yuto17320508 5:1fa5aa097af5 243 pid_->setTarget(target_pose_);
yuto17320508 5:1fa5aa097af5 244 }
yuto17320508 5:1fa5aa097af5 245 void OneLeg::actMotor()
yuto17320508 5:1fa5aa097af5 246 {
yuto17320508 5:1fa5aa097af5 247 motor_->calcDuty(pid_);
yuto17320508 5:1fa5aa097af5 248 motor_->output();
yuto17320508 5:1fa5aa097af5 249 }
yuto17320508 5:1fa5aa097af5 250 void OneLeg::actCylinder()
yuto17320508 5:1fa5aa097af5 251 {
yuto17320508 5:1fa5aa097af5 252 //上がっている場合は下げ、下がっている時は上げる
yuto17320508 5:1fa5aa097af5 253 air1_->changeState();
yuto17320508 5:1fa5aa097af5 254 air2_->changeState();
yuto17320508 5:1fa5aa097af5 255 }
yuto17320508 5:1fa5aa097af5 256
yuto17320508 5:1fa5aa097af5 257 void Robot::setLeg(OneLeg *rightLeg, OneLeg *leftLeg)
yuto17320508 5:1fa5aa097af5 258 {
yuto17320508 5:1fa5aa097af5 259 rightLeg_ = rightLeg;
yuto17320508 5:1fa5aa097af5 260 leftLeg_ = leftLeg;
yuto17320508 5:1fa5aa097af5 261 }
yuto17320508 5:1fa5aa097af5 262 void Robot::setTickerTime(float ticker_time)
yuto17320508 5:1fa5aa097af5 263 {
yuto17320508 5:1fa5aa097af5 264 ticker_time_ = ticker_time;
yuto17320508 5:1fa5aa097af5 265 rightLeg_->pid_->setTimeDelta(ticker_time_);
yuto17320508 5:1fa5aa097af5 266 leftLeg_->pid_->setTimeDelta(ticker_time_);
yuto17320508 5:1fa5aa097af5 267 }
yuto17320508 5:1fa5aa097af5 268 void Robot::setAirWaitTime(float air_wait_time)
yuto17320508 5:1fa5aa097af5 269 {
yuto17320508 5:1fa5aa097af5 270 air_wait_time_ = air_wait_time;
yuto17320508 5:1fa5aa097af5 271 DEBUG("set AirWaitTime: %.3f\n\r", air_wait_time_);
yuto17320508 5:1fa5aa097af5 272 }
yuto17320508 5:1fa5aa097af5 273 void Robot::run()
yuto17320508 5:1fa5aa097af5 274 {
yuto17320508 6:75cfa1a66382 275 while (/*!rightLeg_->pid_->IsConvergence_ || */!leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/
yuto17320508 5:1fa5aa097af5 276 {
yuto17320508 5:1fa5aa097af5 277 //ticker_time毎にモータに出力する
yuto17320508 5:1fa5aa097af5 278 float time_s = timer.read();
yuto17320508 5:1fa5aa097af5 279 rightLeg_->actMotor();
yuto17320508 5:1fa5aa097af5 280 leftLeg_->actMotor();
yuto17320508 5:1fa5aa097af5 281 float rest_time_s = ticker_time_ - (timer.read() - time_s);
yuto17320508 5:1fa5aa097af5 282 //ticker_timeまで待機
yuto17320508 5:1fa5aa097af5 283 if (rest_time_s > 0)
yuto17320508 6:75cfa1a66382 284 {
yuto17320508 5:1fa5aa097af5 285 wait(rest_time_s);
yuto17320508 6:75cfa1a66382 286 DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
yuto17320508 6:75cfa1a66382 287 }
yuto17320508 6:75cfa1a66382 288
yuto17320508 5:1fa5aa097af5 289 else //時間が足りない場合警告
yuto17320508 6:75cfa1a66382 290 printf("error: restTime not enough\n\r");
yuto17320508 5:1fa5aa097af5 291 DEBUG("loop end\n\r")
yuto17320508 5:1fa5aa097af5 292 }
yuto17320508 5:1fa5aa097af5 293
yuto17320508 5:1fa5aa097af5 294 rightLeg_->actCylinder();
yuto17320508 5:1fa5aa097af5 295 leftLeg_->actCylinder();
yuto17320508 5:1fa5aa097af5 296 wait(air_wait_time_);
yuto17320508 5:1fa5aa097af5 297 }
eri 0:111abd91b0cb 298
eri 0:111abd91b0cb 299
eri 0:111abd91b0cb 300
eri 0:111abd91b0cb 301
eri 0:111abd91b0cb 302
eri 0:111abd91b0cb 303
eri 0:111abd91b0cb 304 void setup();
eri 0:111abd91b0cb 305 void wait_MR1();
eri 3:df0c9883e403 306
yuto17320508 5:1fa5aa097af5 307 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
yuto17320508 5:1fa5aa097af5 308 //しかし変更を多々行うためポインタ渡しにしてある
yuto17320508 5:1fa5aa097af5 309 //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
yuto17320508 6:75cfa1a66382 310 PIDcontroller pid_r(0.002, 0.000, 0.000);
yuto17320508 6:75cfa1a66382 311 PIDcontroller pid_l(0.002, 0.000, 0.000); //Kp.Ki,Kd
yuto17320508 5:1fa5aa097af5 312 Motor motorLeft(&motor_l_f, &motor_l_b),
yuto17320508 5:1fa5aa097af5 313 motorRight(&motor_r_f, &motor_r_b); //forward,backのピンを代入
yuto17320508 5:1fa5aa097af5 314 OneLeg leftLeg, rightLeg;
yuto17320508 5:1fa5aa097af5 315 Robot robot;
eri 0:111abd91b0cb 316
eri 0:111abd91b0cb 317
eri 3:df0c9883e403 318 int main()
eri 3:df0c9883e403 319 {
yuto17320508 5:1fa5aa097af5 320 DEBUG("start\n\r");
eri 3:df0c9883e403 321 setup();
yuto17320508 6:75cfa1a66382 322
yuto17320508 5:1fa5aa097af5 323 //PIDコントローラ生成　左右２つで、係数の変更はメンバ関数を用いて行う
yuto17320508 5:1fa5aa097af5 324 //許容誤差設定
yuto17320508 5:1fa5aa097af5 325 pid_r.setTolerance(20.0);
yuto17320508 5:1fa5aa097af5 326 pid_l.setTolerance(20.0);
yuto17320508 5:1fa5aa097af5 327 //モータ生成　PID、ECの代入を行うこと
yuto17320508 5:1fa5aa097af5 328 motorLeft.setEncoder(&ec_l);
yuto17320508 5:1fa5aa097af5 329 motorRight.setEncoder(&ec_r);
yuto17320508 5:1fa5aa097af5 330
yuto17320508 5:1fa5aa097af5 331 AirCylinder air_leg[4] = {//名前は分けたほうがいいきがする？
yuto17320508 5:1fa5aa097af5 332 AirCylinder(&air[0]),
yuto17320508 5:1fa5aa097af5 333 AirCylinder(&air[1]),
yuto17320508 5:1fa5aa097af5 334 AirCylinder(&air[2]),
yuto17320508 5:1fa5aa097af5 335 AirCylinder(&air[3])};
yuto17320508 5:1fa5aa097af5 336 AirCylinder air_nonleg[2] = {
yuto17320508 5:1fa5aa097af5 337 AirCylinder(&air[4]),
yuto17320508 5:1fa5aa097af5 338 AirCylinder(&air[5])};
eri 3:df0c9883e403 339
yuto17320508 5:1fa5aa097af5 340 //シリンダ初期設定
yuto17320508 5:1fa5aa097af5 341 air_leg[0].open();
yuto17320508 5:1fa5aa097af5 342 air_leg[1].close();//内側の二つが開く
yuto17320508 5:1fa5aa097af5 343 air_leg[2].close();
yuto17320508 5:1fa5aa097af5 344 air_leg[3].open();
yuto17320508 5:1fa5aa097af5 345
yuto17320508 5:1fa5aa097af5 346 leftLeg.setMotor(&motorLeft);
yuto17320508 5:1fa5aa097af5 347 leftLeg.setPIDcontroller(&pid_l);
yuto17320508 5:1fa5aa097af5 348 leftLeg.setAir(&air_leg[0], &air_leg[1]);
yuto17320508 5:1fa5aa097af5 349 rightLeg.setMotor(&motorRight);
yuto17320508 5:1fa5aa097af5 350 rightLeg.setPIDcontroller(&pid_r);
yuto17320508 5:1fa5aa097af5 351 rightLeg.setAir(&air_leg[2], &air_leg[3]);
eri 3:df0c9883e403 352
yuto17320508 5:1fa5aa097af5 353 robot.setLeg(&rightLeg, &leftLeg);
yuto17320508 5:1fa5aa097af5 354 robot.setTickerTime(0.01); //モータ出力間隔 0.01
yuto17320508 5:1fa5aa097af5 355 robot.setAirWaitTime(0.2); //シリンダを待つ時間
yuto17320508 6:75cfa1a66382 356
yuto17320508 6:75cfa1a66382 357 motorLeft.setDutyLimit(0.2);
yuto17320508 6:75cfa1a66382 358 motorRight.setDutyLimit(0.2);
eri 3:df0c9883e403 359
yuto17320508 5:1fa5aa097af5 360 //初期位置は0これは内側が一番内側の時
yuto17320508 6:75cfa1a66382 361 char str[255] = {};
yuto17320508 6:75cfa1a66382 362 printf("setup complete Input any key\n\r");
yuto17320508 6:75cfa1a66382 363 scanf("%s", str);
yuto17320508 6:75cfa1a66382 364 printf("start!");
yuto17320508 6:75cfa1a66382 365 leftLeg.setTargetPose(2000.0);
yuto17320508 6:75cfa1a66382 366 rightLeg.setTargetPose(2000.0);
yuto17320508 5:1fa5aa097af5 367 robot.run();
yuto17320508 5:1fa5aa097af5 368 leftLeg.setTargetPose(80.0);
yuto17320508 5:1fa5aa097af5 369 rightLeg.setTargetPose(80.0);
yuto17320508 5:1fa5aa097af5 370 robot.run();
yuto17320508 6:75cfa1a66382 371 }
yuto17320508 5:1fa5aa097af5 372 //
yuto17320508 5:1fa5aa097af5 373 DEBUG("program end\n\n\r");
eri 0:111abd91b0cb 374 }
eri 0:111abd91b0cb 375
eri 0:111abd91b0cb 376 void setup()
eri 0:111abd91b0cb 377 {
yuto17320508 6:75cfa1a66382 378 motor_r_f.period_us(50);
yuto17320508 6:75cfa1a66382 379 motor_r_b.period_us(50);
yuto17320508 6:75cfa1a66382 380 motor_l_f.period_us(50);
yuto17320508 6:75cfa1a66382 381 motor_l_b.period_us(50);
yuto17320508 5:1fa5aa097af5 382
yuto17320508 5:1fa5aa097af5 383 for (int i = 0; i < 4; i++)
yuto17320508 5:1fa5aa097af5 384 {
yuto17320508 5:1fa5aa097af5 385 air[i] = 1;
eri 3:df0c9883e403 386 }
yuto17320508 5:1fa5aa097af5 387 air[4] = 0;
yuto17320508 5:1fa5aa097af5 388 air[5] = 0;
yuto17320508 5:1fa5aa097af5 389
eri 0:111abd91b0cb 390 servo.init();
eri 2:90bdca5d5b60 391 wait_us(1000);
yuto17320508 5:1fa5aa097af5 392 servo.set_degree(0, 105); //初期角度105
eri 3:df0c9883e403 393
yuto17320508 5:1fa5aa097af5 394 device.baud(115200);
yuto17320508 5:1fa5aa097af5 395 device.format(8, Serial::None, 1);
yuto17320508 5:1fa5aa097af5 396 device.attach(dev_rx, Serial::RxIrq);
eri 3:df0c9883e403 397
yuto17320508 5:1fa5aa097af5 398 wait(0.05);
yuto17320508 5:1fa5aa097af5 399 theta0 = degree0;
eri 3:df0c9883e403 400 }
eri 3:df0c9883e403 401 void wait_MR1()
eri 3:df0c9883e403 402 {
yuto17320508 5:1fa5aa097af5 403 while (switch_x == 1)
yuto17320508 5:1fa5aa097af5 404 {
kageyuta 1:3ae63be5592b 405 }
kageyuta 1:3ae63be5592b 406 //while(switch_hand==0);
eri 3:df0c9883e403 407 //air_hand=1;
eri 0:111abd91b0cb 408 wait(1);
yuto17320508 5:1fa5aa097af5 409 }