电机控制与缓动函数

randoms

问题

在机器人的控制中最最基本的就是电机的控制。以舵机为例，最简单的控制方法就是直接给想要控制的舵机发送目标角度。但是我们实际会发现这样会有很多问题。比如舵机的动作速度没办法控制。动作看起来非常的生硬，直来直去。这样的控制也会比较影响舵机的使用寿命。这样就会出现一个问题，如何能够流畅的控制电机呢？

首先什么样的动作才叫做流畅呢？

同样是左右来回运动的两个方块，第二块的运动方式就要比第一块流畅许多。第一块只是一恒定大小的速度在运动，在两个边缘处的速度是突变的。这样对电机的伤害就很大。第二块的运动包含了加速和减速的过程，整过过程总电机的力相对要更均匀些。

如何选取缓动函数

现在我们已经知道了应该在运动中加入适当的过渡变化。那么如何选取合适的过度函数呢？
这里实际上可以考虑成一个常见的物理运动过程。比如一个有阻尼的弹簧振子的运动。

根据阻尼大小的不同可以分为欠阻尼，过阻尼和临界阻尼三种状态。这些状态都运动特点如下图所示

假如我们想要把一个运动的电机停下来。按照阻尼运动来进行控制就是一个比较好的方式。但是在实际开发过程中，我们肯定不能直接使用上图的这种运动方程，因为算起来会比较复杂。在计算机绘图程序中经常使用贝塞尔函数来绘制平滑的曲线。利用这个函数就可以近似的模拟出以上的几个运动方程。

比如欠阻尼的情况
0_1446100961675_QQ截图20151029144214.png
又比如过阻尼的情况
0_1446101025205_QQ截图20151029144335.png

具体怎么实现呢

这个具体的实现过程和一般程序的动画控制函数差不多。下面以java为例来分析具体应该如何实现。
一半的动画函数都包含以下几个参数，初始值，结束值，时间函数（这个就是用来控制变化曲线的），更新函数。下面是我的code

public static ActionStatusController actionAnimation(final int start, final int end, final int duration, final AnimationTimeFunction timeFunction, final AnimationHandler controlFunction){
    final ThreadUtils mAnimationThread = new ThreadUtils(new OnThreadStartHandler(){
        @Override
        public void OnStart() {
            super.OnStart();
            int changes = end - start;
            int time = 0;
            while (time < duration){
                controlFunction.onProgress((int)(start + changes * timeFunction.getInterpolation((float)time/duration)));
                try{
                    Thread.sleep(40);
                    time += 40;
                }catch (Exception e){
                    e.printStackTrace();
                    break; //action was canceled
                }
            }
        }
    });
    ActionStatusController mController = new ActionStatusController(mAnimationThread);
    return mController;
}

每次根据时间算出变化量，然后再把变化量传入到更新函数里面去进行具体的操作。
因为一个动作可能包含了一些列电机的很多行为，所以为了使用方便可以包装出一个动作控制类。

package com.novelor.servicebot.servicebot.models;

import android.util.Log;

import com.novelor.servicebot.servicebot.callbacks.OnThreadStartHandler;
import com.novelor.servicebot.servicebot.utils.ArrayUtils;
import com.novelor.servicebot.servicebot.utils.ThreadUtils;

import java.util.Collection;

/**
 * Created by randoms on 2015/10/23.
 */
public class ActionStatusController {

    private ThreadUtils mAction;
    private boolean startFlag = false;

    public boolean isFinished(){
        return !mAction.isAlive();
    }

    public ActionStatusController (ThreadUtils mAction){
        this.mAction = mAction;
    }

    public void start(){

        startFlag = true;
        mAction.start();
    }

    public void cancel(){
        this.mAction.interrupt();
    }

    public ActionStatusController then(ActionStatusController next){
        while (!isFinished() || !startFlag){
            try{
                Thread.sleep(5);
            }catch (Exception e){e.printStackTrace();}
        }
        next.start();
    return next;
}

public ActionStatusController then(ActionStatusController[] nexts){
    Log.d("Bot", "here1");
    while (!isFinished() || !startFlag){
        try{
            Thread.sleep(5);
        }catch (Exception e){e.printStackTrace();}
    }
    Log.d("Bot", "here2");
    for(int i = 0;i<nexts.length;i++){
        nexts[i].start();
    }
    // wait all actions to finish
    for(int i=0;i<nexts.length;i++){
        while (!nexts[i].isFinished() || !nexts[i].startFlag){
            try{
                Thread.sleep(5);
            }catch (Exception e){e.printStackTrace();}
        }
    }
    ActionStatusController res =  new ActionStatusController(new ThreadUtils(new OnThreadStartHandler(){
        @Override
        public void OnStart() {
            super.OnStart();
        }
    }));
    res.start();
    return res;
}

public static ActionStatusController Start(ActionStatusController[] actions){
        Log.d("Bot", "before start");
        for(int i = 0;i<actions.length;i++){
            actions[i].start();
        }
        Log.d("Bot", "started");
        for(int i=0;i<actions.length;i++){
            while (!actions[i].isFinished() || !actions[i].startFlag){
                try{
                    Thread.sleep(5);
                }catch (Exception e){e.printStackTrace();}
            }
        }
        ActionStatusController res =  new ActionStatusController(new ThreadUtils(new OnThreadStartHandler(){
            @Override
            public void OnStart() {
                super.OnStart();
            }
        }));
        res.start();
        return res;
    }
}

这样在实际使用中就可以这样了

ActionStatusController.Start(reset()).then(new ActionStatusController[]{
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 194, 500, 1),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 180, 500, 2),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 180, 500, 3),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 158, 500, 4),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 25, 500, 5),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 3, 500, BotApi.STEER_RIGHT_HAND)
            }).then(new ActionStatusController[]{
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 194, 500, 1),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 140, 500, 2),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 172, 500, 3),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 500, 3, BotApi.STEER_RIGHT_HAND),
            }).then(
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 39, 300, BotApi.STEER_RIGHT_HAND) // 抓
            ).then(
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 165, 300, 3)
            ).then(new ActionStatusController[]{
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 237, 500, 1),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 144, 500, 3),
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 0, 500, 5)
            }).then(
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 12, 300, BotApi.STEER_RIGHT_HAND)
            ).then(
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 172, 300, 2)
            ).then(
                    AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 93, 300, 5)
            ).then(
                    reset()
            )

/**
 *
 * @param start
 * @param end
 * @param duration
 * @param index index start from 1
 * @return
 */
public static ActionStatusController AnimateSteer(int start, int end, int duration, final int index){
    if(start == BotApi.STEER_ANGEL_DEFAULT){
        start = BotApi.getDefaultAngles()[BotApi.jointMapping[index -1] - 1];
    }
    if(start == BotApi.STEER_ANGEL_CURRENT){
        start = BotApi.getCurrentAngles()[BotApi.jointMapping[index -1] - 1];
    }
    if(end == BotApi.STEER_ANGEL_DEFAULT){
        end = BotApi.getDefaultAngles()[BotApi.jointMapping[index -1] - 1];
    }
    if(end == BotApi.STEER_ANGEL_CURRENT){
        end = BotApi.getCurrentAngles()[BotApi.jointMapping[index -1] - 1];
    }
    return actionAnimation(start, end, duration, new Cubicbezier(0.445, 0.05, 0.55, 0.95), new AnimationHandler(){
        @Override
        public void onProgress(int value) {
            super.onProgress(value);
            BotApi.SetSteer(BotApi.jointMapping[index - 1], value);
        }
    });
}

其中Cubicbezier(0.445, 0.05, 0.55, 0.95)就是缓动函数

package com.novelor.servicebot.servicebot.utils;

import android.graphics.PointF;

import com.novelor.servicebot.servicebot.models.AnimationTimeFunction;

/**
 * Created by randoms on 2015/10/23.
 */
public class Cubicbezier implements AnimationTimeFunction {

    protected PointF start;
    protected PointF end;
    protected PointF a = new PointF();
    protected PointF b = new PointF();
    protected PointF c = new PointF();

    public Cubicbezier(PointF start, PointF end) throws IllegalArgumentException {
        if (start.x < 0 || start.x > 1) {
            throw new IllegalArgumentException("startX value must be in the range [0, 1]");
        }
        if (end.x < 0 || end.x > 1) {
            throw new IllegalArgumentException("endX value must be in the range [0, 1]");
        }
        this.start = start;
        this.end = end;
    }

    public Cubicbezier(float startX, float startY, float endX, float endY) {
        this(new PointF(startX, startY), new PointF(endX, endY));
    }

    public Cubicbezier(double startX, double startY, double endX, double endY) {
        this((float) startX, (float) startY, (float) endX, (float) endY);
    }

    /**
     *
     * @param time between 0 and 1
     * @return between 0 and 1
     */
    @Override
    public float getInterpolation(float time) {
        return getBezierCoordinateY(getXForTime(time));
    }

    protected float getBezierCoordinateY(float time) {
        c.y = 3 * start.y;
        b.y = 3 * (end.y - start.y) - c.y;
        a.y = 1 - c.y - b.y;
        return time * (c.y + time * (b.y + time * a.y));
    }

    protected float getXForTime(float time) {
        float x = time;
        float z;
        for (int i = 1; i < 14; i++) {
            z = getBezierCoordinateX(x) - time;
        if (Math.abs(z) < 1e-3) {
            break;
        }
        x -= z / getXDerivate(x);
    }
    return x;
}

private float getXDerivate(float t) {
    return c.x + t * (2 * b.x + 3 * a.x * t);
}

private float getBezierCoordinateX(float time) {
    c.x = 3 * start.x;
    b.x = 3 * (end.x - start.x) - c.x;
    a.x = 1 - c.x - b.x;
    return time * (c.x + time * (b.x + time * a.x));
}
}

暂搁于此，以后再优化补充。

realme

我找YouTube上找到了解决方案，大家可以参考一下
https://youtu.be/pcfJJxv6q1s?si=5W3cxVrD6KBp3Kfx
https://youtu.be/fC9uxdOBhfA?si=8ETIvuGc4ji6PzKs
https://youtu.be/T0bOZr-D6vs?si=tkZTFNhMPjpvStmY

realme

@realme https://easings.net/zh-cn#