nakashima Kohei
足回り動かすためのライブラリ
使用例
#include "scrp_slave.hpp"
#include "core.hpp"
#include "mbed.h"
ScrpSlave sendpwm(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
Core RBT(&AKASHIKOSEN,OMNI4,0.02);
int main(){
/*--------------SETUP--------------*/
AKASHIKOSEN.setCWID(0,1,2,3);
AKASHIKOSEN.setSWID(4,5,6,7);
RBT.addENC(PC_4,PA_13,512,4,0);
RBT.addENC(PA_14,PA_15,512,4,1);
RBT.addENC(PC_2,PC_3,512,4,2);
RBT.addENC(PC_10,PC_11,512,4,3);
RBT.addENC(PA_7,PA_6,512,4,4);
RBT.addENC(PA_9,PA_8,512,4,5);
RBT.addENC(PC_1,PC_0,512,4,6);
RBT.addENC(PC_5,PA_12,512,4,7);
RBT.START();
/*--------------LOOP--------------*/
Position pos;
while(true){
pos = RBT.getStatus();
printf("x:%lf,y:%lf,theta:%lf\n",pos.x,pos.y,pos.theta);
RBT.LOOP();
}
}
Diff: core.hpp
- Revision:
- 2:d88ff6dda390
- Parent:
- 1:59244694c2bb
- Child:
- 6:87fd489a9801
--- a/core.hpp Tue Sep 21 06:43:07 2021 +0000
+++ b/core.hpp Wed Oct 13 08:12:34 2021 +0000
@@ -1,6 +1,8 @@
#ifndef take_core
#define take_core
+#include "scrp_slave.hpp"
+
#include <vector>
#include "mbed.h"
#include "motor.hpp"
@@ -16,13 +18,13 @@
/*INFO
*Robot ROBOTNAME(四つ角のタイヤの半径(mm),計測輪の半径(mm),中心から角までの距離(mm),中心からフレームまでの距離(mm));
*Core rbt(&ROBOTNAME,一周期の時間(s);
-*rbt.addMOT(正転ピン,逆転ピン,2048,ID(0~3));
+*rbt.addMOT(正転ピン,逆転ピン,2048,最大値,最小値,ID(0~3));
*rbt.addENC(正転ピン,逆転ピン,分解能,モード(1~4),ID(0~7));
-*rbt.addPID(Kp,Ki,Kd,ID(0~7));
+*rbt.addPID(Kp,Ki,Kd,最大値,最小値,ID(0~7));
*ROBOTNAME.setCWID(右前輪,右後輪,左後輪,左前輪のID);
*ROBOTNAME setSWID(前方,右方,後方,左方計測輪のID);
*IDは対応するもの同士で合わせてください
-*rbt.setLIM(pwm最大,最小,PID最大,最小);
+*addMOTやaddPIDのあとに->setLimit(max,min)をつけると値を制限できます
*rbt.START();ロボットの動作開始
*rbt.LOOP();一周期の時間固定のため、ループの最後に書いてください
*rbt.WAIT(時間(us));ループ内の時間がその時間になるまで待機します
@@ -34,20 +36,25 @@
class Core{
public:
Core(Robot* robot,int mode,double dt);
- void addMOT(PinName plus,PinName minus,int period,int id);
+ Motor* addMOT(PinName plus,PinName minus,int period,int id);
void addENC(PinName plus,PinName minus,int resolution,int mode,int id);
- void addPID(double Kp,double Ki,double Kd,int id);
- void setLIM(double MOT_max,double MOT_min,double PID_max,double PID_min);
+ PID* addPID(double Kp,double Ki,double Kd,int id);
+ void setVelocity(double Vx,double Vy,double Vw);
+ void setPWM(double pwm,int id);
void START();
bool LOOP();
void WAIT(double wt);
- void setVelocity(double Vx,double Vy,double Vw);
Position getStatus();
Timer timer;
Robot* rbt;
std::vector<Motor*> Mots;
std::vector<Encoder*> Encs;
std::vector<PID*> PIDs;
+ ScrpSlave* scrp;
+ Core(Robot* robot,ScrpSlave* scrp,int mode,double dt);
+ void sendPWM(double pwm,int id);
+ void sendVelocity(double Vx,double Vy,double Vw);
+ void setPosition(double x,double y,double theta);
private:
int mode;
double dt;