変形CSL関数

csl_motor.m

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