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