Skip to content

Instantly share code, notes, and snippets.

@afjk

afjk/csl_motor.m

Last active Aug 29, 2015
Embed
What would you like to do?
変形CSL関数
- (void) doMonitoring{
// Δφ0(t)=-gi/K*Δφ1(t)+gf*Δφ0(t-1)
const float gi = 0.5f;
const float gf = 1.4f;
const float Kinv = 10.0f;
const float MAX_VOL = 2.0f; //角速度最大値
// 現在の角度
btScalar angle = pHinge->getHingeAngle();
// btDiscreteDynamicsWorld::stepSimulationのmaxSubStepsを1.0で指定している場合
// 6stepで100msec
if( cnt % 6 == 0 ){
dt = angle - preAngle; // 角度差分の計測値:Δφ1(t)の1/10
if( dt < -3.14 ){
dt += 6.28;
}else if( dt > 3.14 ){
dt -= 6.28;
}
// 前回指定した目標角度差分:Δφ0(t-1)の1/10
btScalar preDiff =vel/10.0f;
// 変形CSL式 diff:Δφ0(t)の1/10
diff = -gi*Kinv*dt + gf*preDiff;
// モーターに指定する回転角速度(=目標角度差分Δφ(t))
vel = diff*10.0f;
// 上限値設定
if( vel > MAX_VOL ) vel = MAX_VOL;
if( vel < -MAX_VOL ) vel = -MAX_VOL;
NSLog(@"diff %f angle %f vel %f", diff, angle, vel);
preAngle = angle;
}
pHinge->enableAngularMotor(true, vel, 100);
cnt += 1;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment