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