nakashima Kohei / take_mbed

足回り動かすためのライブラリ

使用例

#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();
}
}

Files at this revision

API Documentation at this revision

Revision 11:80800fd9f4af, committed 2021-10-29

Comitter:
hamohamo
Date:
Fri Oct 29 09:20:31 2021 +0000
Parent:
10:f434b6848059
Commit message:
asd

Changed in this revision

core.cpp Show annotated file Show diff for this revision Revisions of this file
imc.cpp Show annotated file Show diff for this revision Revisions of this file
pid.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/core.cpp	Thu Oct 28 10:58:56 2021 +0000
+++ b/core.cpp	Fri Oct 29 09:20:31 2021 +0000
@@ -1,6 +1,6 @@
 #include "core.hpp"
 
-Core::Core(Robot* robot,int mode,double dt):rbt(robot),mode(mode),dt(dt),Mots(5),Encs(9),PIDs(6){
+Core::Core(Robot* robot,int mode,double dt):rbt(robot),mode(mode),dt(dt),Mots(5),Encs(9),PIDs(6),IMCs(6){
     pos.x = 0.0;
     pos.y = 0.0;
     pos.theta = 0.0;
@@ -17,6 +17,7 @@
     return PIDs.at(id);
 }
 IMC* Core::addIMC(double K,double T,int id){
+    printf("%d\n",id);
     IMCs.at(id) = new IMC(K,T,id);
     return IMCs.at(id);
 }
@@ -127,7 +128,7 @@
     vel.theta = vw;
     return pos;
 }
-Core::Core(Robot* robot,ScrpSlave* scrp,int mode,double dt):rbt(robot),scrp(scrp),mode(mode),dt(dt),Mots(5),Encs(9),PIDs(6){}
+Core::Core(Robot* robot,ScrpSlave* scrp,int mode,double dt):rbt(robot),scrp(scrp),mode(mode),dt(dt),Mots(5),Encs(9),PIDs(6),IMCs(6){}
 void Core::sendPWM(double pwm,int id){scrp->send1(255, id ,(pwm * 100.0));}
 void Core::sendVelocity(double Vx,double Vy,double Vw){
     double w1,w2,w3,w4;

--- a/imc.cpp	Thu Oct 28 10:58:56 2021 +0000
+++ b/imc.cpp	Fri Oct 29 09:20:31 2021 +0000
@@ -10,6 +10,8 @@
 }
 void IMC::Update(double Value,double Target,double dt){
     mv = (Target - Value + simulate(x,mv,dt))*(1.0/K);
+    
+    if(Target == 0.0) mv = 0.0;
     if(mv<Min) mv = Min;
     if(mv>Max) mv = Max;
 }

--- a/pid.cpp	Thu Oct 28 10:58:56 2021 +0000
+++ b/pid.cpp	Fri Oct 29 09:20:31 2021 +0000
@@ -22,6 +22,7 @@
     if(addmv) mv += ((Kp*P)+(Ki*I)+(Kd*D));
     else mv = ((Kp*P)+(Ki*I)+(Kd*D));
     e[1] = e[2];
+    if(Target == 0.0) mv = 0.0;
     if(mv<Min) mv = Min;
     if(mv>Max) mv = Max;
     if(integral<Min) integral = Min;