项目需求

最近做一个项目,要求使用虚拟摇杆控制机器人设备前进后退转弯,整个过程的思路不算复杂,写篇文章记录下大致思路
(1)黄色圆不动时候 小车速度为0
(2)拖动摇杆 拖动距离越大 小车速度越大 距离最大为灰色圆环半径
(3)向正上方拖动时候要求左右轮速度相同 小车前进
(4)向正下方拖动时候要求左右轮速度相同 小车后退
(5)向斜方向拖动时候要求小车向对应方向转弯
(6)速度范围1200~1800

一、360虚拟摇杆的实现

中间黄色圆可以360度移动,最大运动距离为外部灰色圆环半径
在这里插入图片描述

思路

以圆心为坐标系原点 分为四个象限 取两个速度 d/R y/R 为左右轮速度
在第一象限运动 手指由左到右运动 小车则向右转弯 右轮速度减小 即此时右轮速度为小的那个y/R 左轮速度为大的那个 d/R
其他象限同理
R为灰色圆环半径,即运动最大距离
在这里插入图片描述

代码实现:

package com.light.robotproject.views; 
 
import android.annotation.SuppressLint; 
import android.content.Context; 
import android.graphics.Bitmap; 
import android.graphics.BitmapFactory; 
import android.graphics.Canvas; 
import android.graphics.Color; 
import android.graphics.Paint; 
import android.graphics.PixelFormat; 
import android.graphics.Point; 
import android.graphics.PorterDuff.Mode; 
import android.graphics.RectF; 
import android.util.AttributeSet; 
import android.util.Log; 
import android.view.MotionEvent; 
import android.view.SurfaceHolder; 
import android.view.SurfaceHolder.Callback; 
import android.view.SurfaceView; 
 
import com.light.robotproject.R; 
import com.light.robotproject.utils.MiscUtil; 
 
public class MySurfaceView2 extends SurfaceView implements Callback {
   
     
 
    private SurfaceHolder sfh; 
    private Canvas canvas; 
    private Paint paint; 
    private Context mContext; 
    private int coordinate; 
    // 固定摇杆背景圆形的半径 
    private int RockerCircleR, SmallRockerCircleR; 
    // 摇杆的X,Y坐标以及摇杆的半径 
    private float SmallRockerCircleX, SmallRockerCircleY; 
 
    private RudderListener listener = null; // 事件回调接口 
 
    public MySurfaceView2(Context context) {
   
     
        super(context); 
    } 
 
    public MySurfaceView2(Context context, AttributeSet as) {
   
     
        super(context, as); 
        this.setKeepScreenOn(true); 
        this.mContext = context; 
        sfh = getHolder(); 
        sfh.addCallback(this); 
        paint = new Paint(); 
        paint.setColor(Color.GREEN); 
        paint.setAntiAlias(true);// 抗锯齿 
        setFocusable(true); 
        setFocusableInTouchMode(true); 
        setZOrderOnTop(true); 
        sfh.setFormat(PixelFormat.TRANSPARENT);// 设置背景透明 
 
    } 
 
    public void surfaceCreated(SurfaceHolder holder) {
   
     
 
        // 获得控件最小值 
        int little = this.getWidth() < this.getHeight() ? this.getWidth() 
                : this.getHeight(); 
        // 根据屏幕大小绘制 
        SmallRockerCircleX = SmallRockerCircleY = coordinate = little / 2; 
        // 固定摇杆背景圆形的半径 
        RockerCircleR = (int) (little * 0.35) - 20; 
        // 摇杆的半径 
        SmallRockerCircleR = (int) (little * 0.15); 
        draw(); 
    } 
 
    /*** 
     * 得到两点之间的弧度 
     */ 
    public double getRad(float px1, float py1, float px2, float py2) {
   
     
        // 得到两点X的距离 
        float x = px2 - px1; 
        // 得到两点Y的距离 
        float y = py1 - py2; 
        // 算出斜边长 
        float xie = (float) Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); 
        // 得到这个角度的余弦值(通过三角函数中的定理 :邻边/斜边=角度余弦值) 
        float cosAngle = x / xie; 
        // 通过反余弦定理获取到其角度的弧度 
        float rad = (float) Math.acos(cosAngle); 
        // 注意:当触屏的位置Y坐标<摇杆的Y坐标我们要取反值-0~-180 
        if (py2 < py1) {
   
     
            rad = -rad; 
        } 
        return rad; 
    } 
 
    public double getAngle(float px1, float py1, float px2, float py2) {
   
     
        double angle = Math.toDegrees(Math.atan2(py2 - py1, px2 - px1)); 
        Log.i("tempRad角度==", angle + ""); 
        return angle; 
 
    } 
 
    public double getDistance(float px1, float py1, float px2, float py2) {
   
     
        // 计算两点间距离公式 
        double juli = Math.sqrt(Math.abs((px2 - px1) * (px2 - px1)) + (py2 - py1) * (py2 - py1)); 
        System.out.println("两点间的距离是:" + juli); 
        if (juli < 0) {
   
     
            juli = -juli; 
        } 
        return juli; 
    } 
 
    @SuppressLint("ClickableViewAccessibility") 
    @Override 
    public boolean onTouchEvent(MotionEvent event) {
   
     
        // 得到摇杆与触屏点所形成的角度 
        double tempRad = getRad(coordinate, coordinate, event.getX(), 
                event.getY()); 
        getAngle(coordinate, coordinate, event.getX(), 
                event.getY()); 
        if (event.getAction() == MotionEvent.ACTION_DOWN 
                || event.getAction() == MotionEvent.ACTION_MOVE) {
   
     
            // 当触屏区域不在活动范围内 
            if (Math.sqrt(Math.pow((coordinate - (int) event.getX()), 2) 
                    + Math.pow((coordinate - (int) event.getY()), 2)) >= RockerCircleR) {
   
     
                // 保证内部小圆运动的长度限制 
                getXY(coordinate, coordinate, RockerCircleR, tempRad); 
            } else {
   
    // 如果小球中心点小于活动区域则随着用户触屏点移动即可 
                SmallRockerCircleX = (int) event.getX(); 
                SmallRockerCircleY = (int) event.getY(); 
            } 
        } else if (event.getAction() == MotionEvent.ACTION_UP) {
   
     
            // 当释放按键时摇杆要恢复摇杆的位置为初始位置 
            SmallRockerCircleX = coordinate; 
            SmallRockerCircleY = coordinate; 
        } 
        draw(); 
        //摇杆移动半径=圆环半径 
        double roundRate = RockerCircleR;//85+15 
 
        //触摸点到圆点的比例 
        double ratio = getDistance(coordinate, coordinate, SmallRockerCircleX, 
                SmallRockerCircleX) / roundRate; 
        //将坐标系旋转 
//        Point nowSmall=calcNewPoint(new Point((int) SmallRockerCircleX,(int) SmallRockerCircleY),new Point(coordinate,coordinate),-45); 
        Point oldSmall = new Point((int) SmallRockerCircleX, (int) SmallRockerCircleY); 
        //将圆点坐标从(150,150)移到中心点(0,0) 
//        double x = Math.abs(oldSmall.x) - coordinate; 
//        double y = Math.abs(oldSmall.y) - coordinate; 
        /** 
         * 获得两个速度 
         * 一个根据圆心到摇杆半径距离比例来算d/r--为大速度 
         * 一个根据y/r来算--为小速度 
         */ 
        double distanceR=getDistance(SmallRockerCircleX,SmallRockerCircleY,coordinate,coordinate)/roundRate; 
        double distanceY=Math.abs(coordinate-SmallRockerCircleY)/roundRate; 
        Log.i("MySurfaceView2==", "dR==" + getDistance(SmallRockerCircleX,SmallRockerCircleY,coordinate,coordinate) + "   dY==" + (coordinate-SmallRockerCircleY) + "\r\ndistanceR==" + distanceR + "distanceY==" + distanceY); 
 
        if (listener != null) {
   
     
            listener.onSteeringWheelChanged(SmallRockerCircleX, 
                    SmallRockerCircleY, distanceR, distanceY); 
        } 
 
        return true; 
    } 
 
    /** 
     * @param R       圆周运动的旋转点 
     * @param centerX 旋转点X 
     * @param centerY 旋转点Y 
     * @param rad     旋转的弧度 
     */ 
    public void getXY(float centerX, float centerY, float R, double rad) {
   
     
        // 获取圆周运动的X坐标 
        SmallRockerCircleX = (float) (R * Math.cos(rad)) + centerX; 
        // 获取圆周运动的Y坐标 
        SmallRockerCircleY = (float) (R * Math.sin(rad)) + centerY; 
        Log.i("MySurfaceView2==getXY", "x==" + SmallRockerCircleX + "y==" + SmallRockerCircleY); 
    } 
 
    public void draw() {
   
     
        try {
   
     
            canvas = sfh.lockCanvas(); 
            // canvas.drawColor(Color.WHITE); 
            canvas.drawColor(Color.TRANSPARENT, Mode.CLEAR);// 清除屏幕 
            drawCircle(); 
            drawRomot(); 
        } catch (Exception e) {
   
     
            // TODO: handle exception 
        } finally {
   
     
            try {
   
     
                if (canvas != null) 
                    sfh.unlockCanvasAndPost(canvas); 
            } catch (Exception e2) {
   
     
 
            } 
        } 
    } 
 
    /** 
     * 绘制圆环 
     */ 
    public void drawCircle() {
   
     
        //绘制圆弧的边界 
        RectF mRectF = new RectF(); 
        mRectF.left = coordinate - RockerCircleR /*- 20*/; 
        mRectF.top = coordinate - RockerCircleR /*- 20*/; 
        mRectF.right = coordinate + RockerCircleR /*+ 20*/; 
        mRectF.bottom = coordinate + RockerCircleR /*+ 20*/; 
        Paint ringNormalPaint = new Paint(paint); 
        ringNormalPaint.setStyle(Paint.Style.STROKE); 
        ringNormalPaint.setStrokeWidth(15); 
        ringNormalPaint.setColor(mContext.getResources().getColor(R.color.Color_584832)); 
        canvas.drawArc(mRectF, 360, 360, false, ringNormalPaint); 
    } 
 
    /** 
     * 绘制摇杆 
     */ 
    public void drawRomot() {
   
     
        paint.setColor(mContext.getResources().getColor(R.color.Color_88FFFF00)); 
        // 绘制摇杆 
        canvas.drawCircle(SmallRockerCircleX, SmallRockerCircleY, 
                SmallRockerCircleR + 10, paint); 
        paint.setColor(Color.YELLOW); 
        // 绘制摇杆 
        canvas.drawCircle(SmallRockerCircleX, SmallRockerCircleY, 
                SmallRockerCircleR - 5, paint); 
 
    } 
 
    public void surfaceChanged(SurfaceHolder holder, int format, int width, 
                               int height) {
   
     
    } 
 
    public void surfaceDestroyed(SurfaceHolder holder) {
   
     
    } 
 
    // 设置回调接口 
    public void setRudderListener(RudderListener rockerListener) {
   
     
        listener = rockerListener; 
    } 
 
    // 回调接口 
    public interface RudderListener {
   
     
        void onSteeringWheelChanged(double x, double y, double distanceR, double distanceY); 
//        void onSteeringWheelChanged(double angle, double distanceRatio); 
    } 
 
    /** 
     * 将点围绕圆点旋转45度  使得x=y点为最北点 
     * 这样当拉到最上面时候  左右轮速度一样 
     * 
     * @param p 
     * @param pCenter 
     * @param angle 
     * @return 
     */ 
    private static Point calcNewPoint(Point p, Point pCenter, float angle) {
   
     
        // calc arc 
        float l = (float) ((angle * Math.PI) / 180); 
 
        //sin/cos value 
        float cosv = (float) Math.cos(l); 
        float sinv = (float) Math.sin(l); 
 
        // calc new point 
        float newX = (float) ((p.x - pCenter.x) * cosv - (p.y - pCenter.y) * sinv + pCenter.x); 
        float newY = (float) ((p.x - pCenter.x) * sinv + (p.y - pCenter.y) * cosv + pCenter.y); 
        return new Point((int) newX, (int) newY); 
    } 
} 
 
   <com.light.robotproject.views.MySurfaceView2 
            android:id="@+id/remote1" 
            android:layout_width="150dp" 
            android:layout_height="150dp" /> 

回调

 remote1.setRudderListener(object : MySurfaceView2.RudderListener {
   
     
            override fun onSteeringWheelChanged( 
                x: Double, 
                y: Double, 
                distanceR: Double, 
                distanceY: Double 
            ) {
   
     
                /** 
                 * 手指触摸地  x,y 
                 * distanceR  触摸点距离圆心distance/灰色圆环半径R 
                 * distanceY  触摸点坐标y/灰色圆环半径R 
                 * 在最北边时候distanceR=distanceY  即左右轮速度相同  设备向前运动 
                 * 根据坐标分为四个象限  根据  x,y和坐标原点的大小  来判断左右转  
                 *  distanceR和distanceY   分别为左右轮速度    具体为哪个速度要根据象限来判断左转右转 
                 * 例如左转时候 右轮速度>左轮速度 
                 */ 
                Log.i( 
                    "RudderListener==", 
                    "x==" + x + "y==" + y + "distanceR==" + distanceR + "distanceY==" + distanceY 
                ) 
                //根据灵敏度算速度 
                getSpeed(x, y, distanceR, distanceY) 
            } 
 
        }) 
 

获取速度

 /** 
     * 根据象限区域判断左右轮速度归属 
     * 圆心为(150,150) 
     */ 
     var minSpeed=1200 
     var maxSpeed=1800 
    fun setSpeed( 
        x: Double, 
        y: Double, 
        distanceR: Double, 
        distanceY: Double, 
        minSpeed: Int, 
        maxSpeed: Int 
    ) {
   
     
        var center = 150  //摇杆中心坐标(150,150) 
        var speedBig = getRealSpeed(distanceR, minSpeed, maxSpeed) 
        var speedSmall = getRealSpeed(distanceY, minSpeed, maxSpeed) 
        if ((x > center && y < center) || (x > center && y > center)) {
   
     //第一、四象限方向  右转、右退  左轮>右轮 
            ch1Speed = speedBig 
            ch2Speed = speedSmall 
        } else if ((x < center && y < center) || (x < center && y > center)) {
   
    //第二、三象限方向 左前、左退  右轮>左轮 
            ch1Speed = speedSmall 
            ch2Speed = speedBig 
        } 
        if (x == 150.0 && y == 150.0) {
   
     
            ch1Speed = 0.0 
            ch2Speed = 0.0 
        } 
        Log.i("获取转速==", "左轮ch1Speed==" + ch1Speed + "右轮ch2Speed==" + ch2Speed) 
    } 

中间踩的坑

刚开始思考这个算法时候觉得很难,我把方向对准在圆上,发现要找规律对我自己这个数学水平来说很难很难,后来在同事的提醒下,将方向转为线性,突然发现茅塞顿开,由此可见处理问题的时候还是不能让自己钻牛角尖,集思广益,自己想不通的东西,别人一句话就搞定了。


评论关闭
IT虾米网

微信公众号号:IT虾米 (左侧二维码扫一扫)欢迎添加!