yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

main.cpp@4:db1640bd0e89, 2019-05-01 (annotated)

Committer:
yuto17320508
Date:
Wed May 01 06:42:07 2019 +0000
Revision:
4:db1640bd0e89
Parent:
3:29999b02e940
Child:
5:28581157108b
lp;

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
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 4:db1640bd0e89 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 4:db1640bd0e89 331 leg_ro.setTargetPose(360+(step_num_l-1)*180);
yuto17320508 4:db1640bd0e89 332 leg_ri.setTargetPose(180+(step_num_l-1)*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 }