足回り動かすためのライブラリ
使用例
#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.cpp
- Revision:
- 7:c0b6afbccd97
- Parent:
- 6:87fd489a9801
- Child:
- 8:475c17b23944
diff -r 87fd489a9801 -r c0b6afbccd97 core.cpp --- a/core.cpp Thu Oct 21 16:16:01 2021 +0000 +++ b/core.cpp Fri Oct 22 07:44:53 2021 +0000 @@ -34,13 +34,15 @@ } void Core::setVelocity(double Vx,double Vy,double Vw){ double w1,w2,w3,w4; - double k = sqrt(2.0)/2.0; + double vx,vy; + vx = cos(pos.theta-M_PI/4.0)*Vx + sin(pos.theta-M_PI/4.0)*Vy; + vy = - sin(pos.theta-M_PI/4.0)*Vx + cos(pos.theta-M_PI/4.0)*Vy; switch(mode){ case OMNI4: - w1 = -Vx*k*(sin(pos.theta)+cos(pos.theta)) - Vy*k*(sin(pos.theta)-cos(pos.theta)) + rbt->C2CD*Vw; - w2 = -Vx*k*(sin(pos.theta)-cos(pos.theta)) + Vy*k*(sin(pos.theta)+cos(pos.theta)) + rbt->C2CD*Vw; - w3 = Vx*k*(sin(pos.theta)+cos(pos.theta)) + Vy*k*(sin(pos.theta)-cos(pos.theta)) + rbt->C2CD*Vw; - w4 = Vx*k*(sin(pos.theta)-cos(pos.theta)) - Vy*k*(sin(pos.theta)+cos(pos.theta)) + rbt->C2CD*Vw; + w1 = -vx + rbt->C2CD*Vw; + w2 = vy + rbt->C2CD*Vw; + w3 = vx + rbt->C2CD*Vw; + w4 = -vy + rbt->C2CD*Vw; break; case OMNI3: w1 = 0.0; @@ -102,8 +104,8 @@ vY = sin(pos.theta)*vx + cos(pos.theta)*vy; } else { - vX = cos(pos.theta-M_PI/2.0)*vx - sin(pos.theta-M_PI/2.0)*vy; - vY = sin(pos.theta-M_PI/2.0)*vx + cos(pos.theta-M_PI/2.0)*vy; + vX = cos(pos.theta-M_PI/4.0)*vx - sin(pos.theta-M_PI/4.0)*vy; + vY = sin(pos.theta-M_PI/4.0)*vx + cos(pos.theta-M_PI/4.0)*vy; } pos.x += (vel.x+vX)*dt/2.0; @@ -118,13 +120,15 @@ 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; - double k = sqrt(2.0)/2.0; + double vx,vy; + vx = cos(pos.theta-M_PI/4.0)*Vx + sin(pos.theta-M_PI/4.0)*Vy; + vy = - sin(pos.theta-M_PI/4.0)*Vx + cos(pos.theta-M_PI/4.0)*Vy; switch(mode){ case OMNI4: - w1 = -Vx*k*(sin(pos.theta)+cos(pos.theta)) - Vy*k*(sin(pos.theta)-cos(pos.theta)) + rbt->C2CD*Vw; - w2 = -Vx*k*(sin(pos.theta)-cos(pos.theta)) + Vy*k*(sin(pos.theta)+cos(pos.theta)) + rbt->C2CD*Vw; - w3 = Vx*k*(sin(pos.theta)+cos(pos.theta)) + Vy*k*(sin(pos.theta)-cos(pos.theta)) + rbt->C2CD*Vw; - w4 = Vx*k*(sin(pos.theta)-cos(pos.theta)) - Vy*k*(sin(pos.theta)+cos(pos.theta)) + rbt->C2CD*Vw; + w1 = -vx + rbt->C2CD*Vw; + w2 = vy + rbt->C2CD*Vw; + w3 = vx + rbt->C2CD*Vw; + w4 = -vy + rbt->C2CD*Vw; break; case OMNI3: w1 = 0.0;