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@4:81f01f93e502, 2019-04-29 (annotated)

Committer:
yuto17320508
Date:
Mon Apr 29 07:01:51 2019 +0000
Revision:
4:81f01f93e502
Parent:
3:7a608fbd3bcd
Child:
5:63462d696256
lslsl

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"
eri 0:c1476d342c13 4
yuto17320508 4:81f01f93e502 5 //#define DEBUG_ON
yuto17320508 4:81f01f93e502 6
yuto17320508 4:81f01f93e502 7 #ifdef DEBUG_ON
yuto17320508 4:81f01f93e502 8 #define DEBUG(...) printf("" __VA_ARGS__);
yuto17320508 4:81f01f93e502 9 #else
yuto17320508 4:81f01f93e502 10 #define DEBUG(...)
yuto17320508 4:81f01f93e502 11 #endif
yuto17320508 4:81f01f93e502 12
yuto17320508 4:81f01f93e502 13 #define Pi 3.14159265359 //円周率π
yuto17320508 4:81f01f93e502 14
yuto17320508 4:81f01f93e502 15 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
yuto17320508 4:81f01f93e502 16
yuto17320508 4:81f01f93e502 17 class PIDcontroller //distanceをvalueに置き換えました
yuto17320508 4:81f01f93e502 18 {
yuto17320508 4:81f01f93e502 19 float Kp_, Ki_, Kd_, tolerance_, time_delta_;
yuto17320508 4:81f01f93e502 20 float pile_, value_old_, target_;
yuto17320508 4:81f01f93e502 21
yuto17320508 4:81f01f93e502 22 public:
yuto17320508 4:81f01f93e502 23 bool IsConvergence_; //収束したかどうか
yuto17320508 4:81f01f93e502 24 PIDcontroller(float Kp, float Ki, float Kd); //初期設定で係数を入力
yuto17320508 4:81f01f93e502 25 void setCoefficients(float Kp, float Ki, float Kd){Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;}; //係数を変更するときに使う
yuto17320508 4:81f01f93e502 26 void setTimeDelta(float delta){time_delta_ = delta;};
yuto17320508 4:81f01f93e502 27 void setTarget(float target); //目標位置の設定
yuto17320508 4:81f01f93e502 28 void setTolerance(float tolerance){tolerance_ = tolerance;}; //許容誤差の設定
yuto17320508 4:81f01f93e502 29 float calc(float nowVal); //現在位置と目標を比較してPID補正
yuto17320508 4:81f01f93e502 30 bool knowConvergence(){return IsConvergence_;}; //収束したかどうかを外部に伝える
yuto17320508 4:81f01f93e502 31 };
yuto17320508 4:81f01f93e502 32
yuto17320508 4:81f01f93e502 33 class Motor //PIDコントローラ、エンコーダを含むモータのクラス
yuto17320508 4:81f01f93e502 34 {
yuto17320508 4:81f01f93e502 35 PwmOut *pin_forward_, *pin_back_;
yuto17320508 4:81f01f93e502 36 Ec *ec_; //対応するエンコーダ
yuto17320508 4:81f01f93e502 37 float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
yuto17320508 4:81f01f93e502 38 int resolution_;
yuto17320508 4:81f01f93e502 39 public:
yuto17320508 4:81f01f93e502 40 Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
yuto17320508 4:81f01f93e502 41 void setDutyLimit(float limit){duty_limit_ = limit;};
yuto17320508 4:81f01f93e502 42 float getPosi(); //ポジをエンコーダから取得
yuto17320508 4:81f01f93e502 43 void calcDuty(PIDcontroller *pid); //Duty比を計算
yuto17320508 4:81f01f93e502 44 void setEncoder(Ec *ec){ec_ = ec;}; //エンコーダを設定
yuto17320508 4:81f01f93e502 45 void setResolution(int reso){resolution_ = reso;};
yuto17320508 4:81f01f93e502 46 void output(); //出力するだけ
yuto17320508 4:81f01f93e502 47 void output(float duty);
yuto17320508 4:81f01f93e502 48 };
yuto17320508 4:81f01f93e502 49
yuto17320508 4:81f01f93e502 50 class OneLeg //足の挙動を制御する
yuto17320508 4:81f01f93e502 51 {
yuto17320508 4:81f01f93e502 52 Motor *motor_;
yuto17320508 4:81f01f93e502 53 float target_pose_;
yuto17320508 4:81f01f93e502 54
yuto17320508 4:81f01f93e502 55 public:
yuto17320508 4:81f01f93e502 56 PIDcontroller *pid_;
yuto17320508 4:81f01f93e502 57 OneLeg(){};
yuto17320508 4:81f01f93e502 58 void setMotor(Motor *motor){motor_ = motor;};
yuto17320508 4:81f01f93e502 59 void setPIDcontroller(PIDcontroller *pid){pid_ = pid;};
yuto17320508 4:81f01f93e502 60 void setTargetPose(float target_pose);
yuto17320508 4:81f01f93e502 61 void actMotor();//モータ出力
yuto17320508 4:81f01f93e502 62 };
yuto17320508 4:81f01f93e502 63
yuto17320508 4:81f01f93e502 64 class Robot
yuto17320508 4:81f01f93e502 65 {
yuto17320508 4:81f01f93e502 66 float ticker_time_, air_wait_time_;
yuto17320508 4:81f01f93e502 67 OneLeg *Leg1_, *Leg2_;
yuto17320508 4:81f01f93e502 68 Timer timer;
yuto17320508 4:81f01f93e502 69
yuto17320508 4:81f01f93e502 70 public:
yuto17320508 4:81f01f93e502 71 Robot(){timer.reset(); timer.start();};
yuto17320508 4:81f01f93e502 72 void setLeg(OneLeg *Leg1_, OneLeg *Leg2_);
yuto17320508 4:81f01f93e502 73 void setTickerTime(float ticker_time);
yuto17320508 4:81f01f93e502 74 void run();//ここがメインで走る記述
yuto17320508 4:81f01f93e502 75 };
yuto17320508 4:81f01f93e502 76
yuto17320508 4:81f01f93e502 77
yuto17320508 4:81f01f93e502 78
yuto17320508 4:81f01f93e502 79 PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
yuto17320508 4:81f01f93e502 80 {
yuto17320508 4:81f01f93e502 81 Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
yuto17320508 4:81f01f93e502 82 DEBUG("set Kp:%.3f KI:%.3f Kd:%.3f \n\r", Kp_, Ki_, Kd_);
yuto17320508 4:81f01f93e502 83 IsConvergence_=true;
yuto17320508 4:81f01f93e502 84 }
yuto17320508 4:81f01f93e502 85 void PIDcontroller::setTarget(float target)
yuto17320508 4:81f01f93e502 86 {
yuto17320508 4:81f01f93e502 87 if (IsConvergence_) //収束時のみ変更可能
yuto17320508 4:81f01f93e502 88 {
yuto17320508 4:81f01f93e502 89 target_ = target;
yuto17320508 4:81f01f93e502 90 DEBUG("set Target: %.3f\n\r", target_);
yuto17320508 4:81f01f93e502 91 IsConvergence_ = false;
yuto17320508 4:81f01f93e502 92 }
yuto17320508 4:81f01f93e502 93 else
yuto17320508 4:81f01f93e502 94 {
yuto17320508 4:81f01f93e502 95 DEBUG("error: setTarget permission denied!\n");
yuto17320508 4:81f01f93e502 96 }
yuto17320508 4:81f01f93e502 97 }
yuto17320508 4:81f01f93e502 98 float PIDcontroller::calc(float nowVal)
yuto17320508 4:81f01f93e502 99 {
yuto17320508 4:81f01f93e502 100 float out = 0;
yuto17320508 4:81f01f93e502 101 //PID計算ここで行う
yuto17320508 4:81f01f93e502 102 float deviation = target_ - nowVal; //目標との差分
yuto17320508 4:81f01f93e502 103 pile_ += deviation; //積分用に和を取る
yuto17320508 4:81f01f93e502 104 out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
yuto17320508 4:81f01f93e502 105 value_old_ = nowVal; //今のデータを保存
yuto17320508 4:81f01f93e502 106 //
yuto17320508 4:81f01f93e502 107 if (fabs(deviation) < tolerance_) //収束した場合
yuto17320508 4:81f01f93e502 108 {
yuto17320508 4:81f01f93e502 109 DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
yuto17320508 4:81f01f93e502 110 out = 0;
yuto17320508 4:81f01f93e502 111 pile_ = 0;
yuto17320508 4:81f01f93e502 112 value_old_ = 0;
yuto17320508 4:81f01f93e502 113 IsConvergence_ = true;
yuto17320508 4:81f01f93e502 114 }
yuto17320508 4:81f01f93e502 115 return out;
yuto17320508 4:81f01f93e502 116 }
yuto17320508 4:81f01f93e502 117
yuto17320508 4:81f01f93e502 118 Motor::Motor(PwmOut *forward, PwmOut *back)
yuto17320508 4:81f01f93e502 119 {
yuto17320508 4:81f01f93e502 120 pin_forward_ = forward;
yuto17320508 4:81f01f93e502 121 pin_back_ = back;
yuto17320508 4:81f01f93e502 122 }
yuto17320508 4:81f01f93e502 123 float Motor::getPosi()
yuto17320508 4:81f01f93e502 124 {
yuto17320508 4:81f01f93e502 125 float posi = 2.0*180*((float)(ec_->getCount)()/(float)resolution_);
yuto17320508 4:81f01f93e502 126
yuto17320508 4:81f01f93e502 127 //DEBUG("value :%d :%d\n\r", (ec_->getCount)(),resolution_);
yuto17320508 4:81f01f93e502 128 DEBUG("posi is %.4f\n\r",posi);
yuto17320508 4:81f01f93e502 129 return posi;
yuto17320508 4:81f01f93e502 130 }
yuto17320508 4:81f01f93e502 131 void Motor::calcDuty(PIDcontroller *pid)
yuto17320508 4:81f01f93e502 132 {
yuto17320508 4:81f01f93e502 133 duty_ = pid->calc(getPosi());
yuto17320508 4:81f01f93e502 134 DEBUG("duty is %.4f\n\r",duty_);
yuto17320508 4:81f01f93e502 135 }
yuto17320508 4:81f01f93e502 136 void Motor::output()
yuto17320508 4:81f01f93e502 137 {
yuto17320508 4:81f01f93e502 138 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 4:81f01f93e502 139 //加速度が一定値を超えたら変更加える
yuto17320508 4:81f01f93e502 140 if (duty_ > 0)
yuto17320508 4:81f01f93e502 141 {
yuto17320508 4:81f01f93e502 142 if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
yuto17320508 4:81f01f93e502 143 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 144 pin_forward_->write(output_duty);
yuto17320508 4:81f01f93e502 145 pin_back_->write(0);
yuto17320508 4:81f01f93e502 146 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 4:81f01f93e502 147 }
yuto17320508 4:81f01f93e502 148 else
yuto17320508 4:81f01f93e502 149 {
yuto17320508 4:81f01f93e502 150 if (pre_duty_ - duty_ > accel_max)
yuto17320508 4:81f01f93e502 151 duty_ = pre_duty_ - accel_max;
yuto17320508 4:81f01f93e502 152 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 153 pin_forward_->write(0);
yuto17320508 4:81f01f93e502 154 pin_back_->write(output_duty);
yuto17320508 4:81f01f93e502 155 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 4:81f01f93e502 156 }
yuto17320508 4:81f01f93e502 157 pre_duty_ = duty_;
yuto17320508 4:81f01f93e502 158 }
yuto17320508 4:81f01f93e502 159 void Motor::output(float duty)
yuto17320508 4:81f01f93e502 160 {
yuto17320508 4:81f01f93e502 161 duty_ = duty;
yuto17320508 4:81f01f93e502 162 //DEBUG("dutyOut %.3f\n\r",duty_);
yuto17320508 4:81f01f93e502 163 //加速度が一定値を超えたら変更加える
yuto17320508 4:81f01f93e502 164 if (duty_ > 0)
yuto17320508 4:81f01f93e502 165 {
yuto17320508 4:81f01f93e502 166 //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
yuto17320508 4:81f01f93e502 167 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 168 pin_forward_->write(output_duty);
yuto17320508 4:81f01f93e502 169 pin_back_->write(0);
yuto17320508 4:81f01f93e502 170 DEBUG("forward %.3f\n\r",pin_forward_->read());
yuto17320508 4:81f01f93e502 171 }
yuto17320508 4:81f01f93e502 172 else
yuto17320508 4:81f01f93e502 173 {
yuto17320508 4:81f01f93e502 174 //if (pre_duty_ - duty_ > accel_max)
yuto17320508 4:81f01f93e502 175 // duty_ = pre_duty_ - accel_max;
yuto17320508 4:81f01f93e502 176 double output_duty=min(fabs(duty_), duty_limit_);
yuto17320508 4:81f01f93e502 177 pin_forward_->write(0);
yuto17320508 4:81f01f93e502 178 pin_back_->write(output_duty);
yuto17320508 4:81f01f93e502 179 DEBUG("back %.3f\n\r",pin_back_->read());
yuto17320508 4:81f01f93e502 180 }
yuto17320508 4:81f01f93e502 181 pre_duty_ = duty_;
yuto17320508 4:81f01f93e502 182 }
yuto17320508 4:81f01f93e502 183
yuto17320508 4:81f01f93e502 184 void OneLeg::setTargetPose(float target_pose)
yuto17320508 4:81f01f93e502 185 {
yuto17320508 4:81f01f93e502 186 target_pose_ = target_pose;
yuto17320508 4:81f01f93e502 187 //PIDにtargetを送る
yuto17320508 4:81f01f93e502 188 pid_->setTarget(target_pose_);
yuto17320508 4:81f01f93e502 189 }
yuto17320508 4:81f01f93e502 190 void OneLeg::actMotor()
yuto17320508 4:81f01f93e502 191 {
yuto17320508 4:81f01f93e502 192 motor_->calcDuty(pid_);
yuto17320508 4:81f01f93e502 193 motor_->output();
yuto17320508 4:81f01f93e502 194 }
yuto17320508 4:81f01f93e502 195
yuto17320508 4:81f01f93e502 196
yuto17320508 4:81f01f93e502 197
yuto17320508 4:81f01f93e502 198 void Robot::setLeg(OneLeg *Leg1, OneLeg *Leg2)
yuto17320508 4:81f01f93e502 199 {
yuto17320508 4:81f01f93e502 200 Leg1_ = Leg1;
yuto17320508 4:81f01f93e502 201 Leg2_ = Leg2;
yuto17320508 4:81f01f93e502 202 }
yuto17320508 4:81f01f93e502 203 void Robot::setTickerTime(float ticker_time)
yuto17320508 4:81f01f93e502 204 {
yuto17320508 4:81f01f93e502 205 ticker_time_ = ticker_time;
yuto17320508 4:81f01f93e502 206 Leg1_->pid_->setTimeDelta(ticker_time_);
yuto17320508 4:81f01f93e502 207 Leg2_->pid_->setTimeDelta(ticker_time_);
yuto17320508 4:81f01f93e502 208 }
yuto17320508 4:81f01f93e502 209 void Robot::run()
yuto17320508 4:81f01f93e502 210 {
yuto17320508 4:81f01f93e502 211 while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) //片方が収束していない時*/
yuto17320508 4:81f01f93e502 212 {
yuto17320508 4:81f01f93e502 213 //ticker_time毎にモータに出力する
yuto17320508 4:81f01f93e502 214 float time_s = timer.read();
yuto17320508 4:81f01f93e502 215 Leg1_->actMotor();
yuto17320508 4:81f01f93e502 216 Leg2_->actMotor();
yuto17320508 4:81f01f93e502 217 float rest_time_s = ticker_time_ - (timer.read() - time_s);
yuto17320508 4:81f01f93e502 218 //ticker_timeまで待機
yuto17320508 4:81f01f93e502 219 if (rest_time_s > 0)
yuto17320508 4:81f01f93e502 220 {
yuto17320508 4:81f01f93e502 221 wait(rest_time_s);
yuto17320508 4:81f01f93e502 222 DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
yuto17320508 4:81f01f93e502 223 }
yuto17320508 4:81f01f93e502 224
yuto17320508 4:81f01f93e502 225 else //時間が足りない場合警告
yuto17320508 4:81f01f93e502 226 printf("error: restTime not enough\n\r");
yuto17320508 4:81f01f93e502 227 DEBUG("loop end\n\r")
yuto17320508 4:81f01f93e502 228 }
yuto17320508 4:81f01f93e502 229
yuto17320508 4:81f01f93e502 230 }
yuto17320508 4:81f01f93e502 231
eri 0:c1476d342c13 232
eri 0:c1476d342c13 233
eri 0:c1476d342c13 234 ////////////関数
yuto17320508 4:81f01f93e502 235 void reset();
eri 0:c1476d342c13 236 void setup();
yuto17320508 4:81f01f93e502 237 void can_send(float target_ro, float target_ri);
eri 0:c1476d342c13 238
eri 0:c1476d342c13 239 ////////////定数
eri 0:c1476d342c13 240
eri 0:c1476d342c13 241 ////////////変数
eri 0:c1476d342c13 242 bool hand_mode=0;
kageyuta 1:86c4c38abe40 243
yuto17320508 4:81f01f93e502 244 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
yuto17320508 4:81f01f93e502 245 //しかし変更を多々行うためポインタ渡しにしてある
yuto17320508 4:81f01f93e502 246 //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
yuto17320508 4:81f01f93e502 247 PIDcontroller pid_lo(0.01, 0.000, 0.000);
yuto17320508 4:81f01f93e502 248 PIDcontroller pid_li(0.01, 0.000, 0.000); //Kp.Ki,Kd
yuto17320508 4:81f01f93e502 249 Motor motor_lo(&motor_lo_f, &motor_lo_b),
yuto17320508 4:81f01f93e502 250 motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
yuto17320508 4:81f01f93e502 251 OneLeg leg_lo, leg_li;
yuto17320508 4:81f01f93e502 252 Robot robot;
eri 0:c1476d342c13 253
eri 0:c1476d342c13 254 /////////////////////////////////////////////
kageyuta 1:86c4c38abe40 255 int main()
kageyuta 1:86c4c38abe40 256 {
eri 0:c1476d342c13 257 setup();
yuto17320508 4:81f01f93e502 258
yuto17320508 4:81f01f93e502 259 pid_lo.setTolerance(10);
yuto17320508 4:81f01f93e502 260 pid_li.setTolerance(10);
yuto17320508 4:81f01f93e502 261
yuto17320508 4:81f01f93e502 262 motor_lo.setEncoder(&ec_lo);
yuto17320508 4:81f01f93e502 263 motor_lo.setResolution(1000);
yuto17320508 4:81f01f93e502 264 motor_li.setEncoder(&ec_li);
yuto17320508 4:81f01f93e502 265 motor_li.setResolution(600);
yuto17320508 4:81f01f93e502 266
yuto17320508 4:81f01f93e502 267 leg_lo.setMotor(&motor_lo);
yuto17320508 4:81f01f93e502 268 leg_lo.setPIDcontroller(&pid_lo);
yuto17320508 4:81f01f93e502 269 leg_li.setMotor(&motor_li);
yuto17320508 4:81f01f93e502 270 leg_li.setPIDcontroller(&pid_li);
yuto17320508 4:81f01f93e502 271
yuto17320508 4:81f01f93e502 272 robot.setLeg(&leg_lo, &leg_li);
yuto17320508 4:81f01f93e502 273 robot.setTickerTime(0.01); //モータ出力間隔 0.01
yuto17320508 4:81f01f93e502 274
yuto17320508 4:81f01f93e502 275 motor_lo.setDutyLimit(0.1);
yuto17320508 4:81f01f93e502 276 motor_li.setDutyLimit(0.1);
yuto17320508 4:81f01f93e502 277
yuto17320508 4:81f01f93e502 278 /*
yuto17320508 4:81f01f93e502 279 char str[255] = {};
yuto17320508 4:81f01f93e502 280 printf("setup complete Input any key\n\r");
yuto17320508 4:81f01f93e502 281 scanf("%s", str);
yuto17320508 4:81f01f93e502 282 printf("start!");
yuto17320508 4:81f01f93e502 283 */
yuto17320508 4:81f01f93e502 284
kageyuta 2:55c616d2e0fe 285 reset();
yuto17320508 4:81f01f93e502 286 printf("bus standby\n\r");
yuto17320508 4:81f01f93e502 287 while(1)
yuto17320508 4:81f01f93e502 288 {
yuto17320508 4:81f01f93e502 289 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 290 }
yuto17320508 4:81f01f93e502 291 printf("bus is %d\n\r", bus_in.read());
yuto17320508 4:81f01f93e502 292
yuto17320508 4:81f01f93e502 293 //Sample
yuto17320508 4:81f01f93e502 294 for(int i = 0; i< 20; ++i)
yuto17320508 4:81f01f93e502 295 {
yuto17320508 4:81f01f93e502 296 can_send(360+i*180,180+i*180);
yuto17320508 4:81f01f93e502 297 leg_lo.setTargetPose(360+i*180);
yuto17320508 4:81f01f93e502 298 leg_li.setTargetPose(180+i*180);
yuto17320508 4:81f01f93e502 299 robot.run();
yuto17320508 4:81f01f93e502 300 motor_lo_f.write(0);
yuto17320508 4:81f01f93e502 301 motor_lo_b.write(0);
yuto17320508 4:81f01f93e502 302 motor_li_f.write(0);
yuto17320508 4:81f01f93e502 303 motor_li_b.write(0);
yuto17320508 4:81f01f93e502 304 while(1)
yuto17320508 4:81f01f93e502 305 {
yuto17320508 4:81f01f93e502 306 if(bus_in.read() == 1) break;
yuto17320508 4:81f01f93e502 307 }
yuto17320508 3:7a608fbd3bcd 308 }
yuto17320508 4:81f01f93e502 309
yuto17320508 4:81f01f93e502 310 motor_lo_f.write(0);
yuto17320508 4:81f01f93e502 311 motor_lo_b.write(0);
yuto17320508 4:81f01f93e502 312 motor_li_f.write(0);
yuto17320508 4:81f01f93e502 313 motor_li_b.write(0);
yuto17320508 4:81f01f93e502 314
eri 0:c1476d342c13 315 }
eri 0:c1476d342c13 316
eri 0:c1476d342c13 317
kageyuta 1:86c4c38abe40 318 void setup()
kageyuta 1:86c4c38abe40 319 {
eri 0:c1476d342c13 320 can1.frequency(1000000);
eri 0:c1476d342c13 321 motor_lo_f.period_us(100);
eri 0:c1476d342c13 322 motor_lo_b.period_us(100);
eri 0:c1476d342c13 323 motor_li_f.period_us(100);
eri 0:c1476d342c13 324 motor_li_b.period_us(100);
kageyuta 1:86c4c38abe40 325
eri 0:c1476d342c13 326 hand.mode(PullUp);
kageyuta 2:55c616d2e0fe 327 switch_lo.mode(PullUp);
kageyuta 2:55c616d2e0fe 328 switch_li.mode(PullUp);
eri 0:c1476d342c13 329 switch4.mode(PullUp);
kageyuta 1:86c4c38abe40 330
eri 0:c1476d342c13 331
eri 0:c1476d342c13 332 device.baud(115200);
eri 0:c1476d342c13 333 device.format(8,Serial::None,1);
eri 0:c1476d342c13 334 device.attach(dev_rx, Serial::RxIrq);
eri 0:c1476d342c13 335 wait(0.05);
eri 0:c1476d342c13 336 theta0=degree0;
eri 0:c1476d342c13 337 check_gyro();
eri 0:c1476d342c13 338 }
eri 0:c1476d342c13 339
eri 0:c1476d342c13 340
eri 0:c1476d342c13 341 //////////////////////////////////////can
yuto17320508 4:81f01f93e502 342 void can_send(float target_ro, float target_ri)
eri 0:c1476d342c13 343 {
kageyuta 1:86c4c38abe40 344 char data[4]= {0};
eri 0:c1476d342c13 345 int target_ro_send=target_ro+360;
eri 0:c1476d342c13 346 int target_ri_send=target_ri+360;
eri 0:c1476d342c13 347 data[0]=target_ro_send & 0b11111111;
eri 0:c1476d342c13 348 data[1]=target_ri_send & 0b11111111;
eri 0:c1476d342c13 349 data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000);
eri 0:c1476d342c13 350 data[3]=hand_mode;
kageyuta 1:86c4c38abe40 351
eri 0:c1476d342c13 352 if(can1.write(CANMessage(0,data,4)))led4=1;
eri 0:c1476d342c13 353 else led4=0;
kageyuta 1:86c4c38abe40 354 }
kageyuta 2:55c616d2e0fe 355 void reset()
kageyuta 2:55c616d2e0fe 356 {
kageyuta 2:55c616d2e0fe 357 while(switch_lo.read()) {
yuto17320508 4:81f01f93e502 358 motor_lo.output(0.07);
kageyuta 2:55c616d2e0fe 359 }
kageyuta 2:55c616d2e0fe 360 ec_lo.reset();
yuto17320508 4:81f01f93e502 361 motor_lo.output(0.0);
kageyuta 2:55c616d2e0fe 362 while(switch_li.read()) {
yuto17320508 4:81f01f93e502 363 motor_li.output(0.07);
kageyuta 2:55c616d2e0fe 364 }
kageyuta 2:55c616d2e0fe 365 ec_li.reset();
yuto17320508 4:81f01f93e502 366 motor_li.output(0.0);
kageyuta 1:86c4c38abe40 367 }
kageyuta 1:86c4c38abe40 368