足回り動かすためのライブラリ
Embed:
(wiki syntax)
Show/hide line numbers
core.hpp
00001 #ifndef take_core 00002 #define take_core 00003 00004 #include "scrp_slave.hpp" 00005 00006 #include <vector> 00007 #include "mbed.h" 00008 #include "motor.hpp" 00009 #include "encoder.hpp" 00010 #include "pid.hpp" 00011 #include "imc.hpp" 00012 #include "robot.hpp" 00013 #include "position.hpp" 00014 00015 #define OMNI4 1 00016 #define OMNI3 2 00017 #define MECANUM 3 00018 00019 /*INFO 00020 *Robot ROBOTNAME(四つ角のタイヤの半径(mm),計測輪の半径(mm),中心から角までの距離(mm),中心からフレームまでの距離(mm)); 00021 *Core rbt(&ROBOTNAME,一周期の時間(s); 00022 *rbt.addMOT(正転ピン,逆転ピン,2048,最大値,最小値,ID(0~3)); 00023 *rbt.addENC(正転ピン,逆転ピン,分解能,モード(1~4),ID(0~7)); 00024 *rbt.addPID(Kp,Ki,Kd,最大値,最小値,ID(0~7)); 00025 *ROBOTNAME.setCWID(右前輪,右後輪,左後輪,左前輪のID); 00026 *ROBOTNAME setSWID(前方,右方,後方,左方計測輪のID); 00027 *IDは対応するもの同士で合わせてください 00028 *addMOTやaddPIDのあとに->setLimit(max,min)をつけると値を制限できます 00029 *rbt.START();ロボットの動作開始 00030 *rbt.LOOP();一周期の時間固定のため、ループの最後に書いてください 00031 *rbt.WAIT(時間(us));ループ内の時間がその時間になるまで待機します 00032 *rbt.setVelocity(速度のx成分,y成分,角速度); 00033 *rbt.getStatus();ロボットの速度と位置を取得します 00034 * 00035 */ 00036 00037 class Core{ 00038 public: 00039 Core(Robot* robot,int mode,double dt); 00040 Motor* addMOT(PinName plus,PinName minus,int period,int id); 00041 void addENC(PinName plus,PinName minus,int resolution,int mode,int id); 00042 PID* addPID(double Kp,double Ki,double Kd,int id); 00043 IMC* addIMC(double K,double T,int id); 00044 void setVelocity(double Vx,double Vy,double Vw); 00045 void setPWM(double pwm,int id); 00046 void START(); 00047 bool LOOP(); 00048 void WAIT(double wt); 00049 Position getStatus(bool onground = true); 00050 Timer timer; 00051 Robot* rbt; 00052 std::vector<Motor*> Mots; 00053 std::vector<Encoder*> Encs; 00054 std::vector<PID*> PIDs; 00055 std::vector<IMC*> IMCs; 00056 ScrpSlave* scrp; 00057 Core(Robot* robot,ScrpSlave* scrp,int mode,double dt); 00058 void sendPWM(double pwm,int id); 00059 void sendVelocity(double Vx,double Vy,double Vw); 00060 void setPosition(double x,double y,double theta); 00061 private: 00062 int mode; 00063 double dt; 00064 unsigned long long t; 00065 Position pos,vel; 00066 }; 00067 00068 #endif
Generated on Thu Jul 14 2022 14:37:03 by 1.7.2