项目需求
 
最近做一个项目，要求使用虚拟摇杆控制机器人设备前进后退转弯，整个过程的思路不算复杂，写篇文章记录下大致思路
 （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)
    }
 
中间踩的坑
 
刚开始思考这个算法时候觉得很难，我把方向对准在圆上，发现要找规律对我自己这个数学水平来说很难很难，后来在同事的提醒下，将方向转为线性，突然发现茅塞顿开，由此可见处理问题的时候还是不能让自己钻牛角尖，集思广益，自己想不通的东西，别人一句话就搞定了。

上篇文章搞定了虚拟摇杆以及左右轮速度算法，接下来就是连接硬件以及向硬件提交信息
 
获得串口通信串儿
 
（1）串口通讯
 （2）通讯规则：
 1、协议格式：（8字节）
 [startbyte] [data1][data2]…[data22][flags][endbyte]
 startbyte=0x0f;
 endbyte=0x00;
 flags标志位我没有用到；
 data1…data22:对应16个通道（ch1-ch16），每个通道11bit
 
2、需要的是data数据 22*8位
 [startbyte] [data1] [data2] … [data22] [flags][endbyte]
 startbyte = 11110000 b (0xF0)
 
//228==1611
 [data1] [data2] … [data22] = [ch1,11bit] [ch2,11bit] … [ch16,11bit]（ch＃= 0 bis ）
 
需要16个通道 规定ch1=左轮速度，ch2=右轮速度，ch5=武器速度
 [ch1,11bit] [ch2,11bit] … [ch16,11bit]（ch＃= 0 bis ）
 通道为11位二进制数据
 例如1200对应的二进制通道数据为10010110000
 
获取对应的data串儿代码
 
public static void main(code1Int:Int，code2Int:Int,code5Int:Int) throws IOException {
		String code = "00000000000";
		String code1 = Integer.toBinaryString(code1Int); //通道1--二进制
		String code2 = Integer.toBinaryString(code2Int); //通道2
		String code5 = Integer.toBinaryString(code5Int); //通道5
		System.out.println(code1);
		System.out.println(code2);
		System.out.println(code5);
		Integer i = 0;
		String io = code1 + code2;
		while(i < 14){
			i++;
			if(i.equals(2)){
				io += code5;
				continue;
			}
			io += code;
		}
		System.out.println(io);
		i = 0;
		Integer beginIndex = 0;
		String[] str = new String[22];
		while(i < 22){
			Integer endIndex = beginIndex + 8;
			str[i] = io.substring(beginIndex, endIndex);
			beginIndex = endIndex;
			i++;
		}
		System.out.println(Arrays.toString(str));
		String endString = "";
		for(String c : str){
			Integer six = Integer.parseInt(c, 2);
			String ipi = Integer.toHexString(six);
			if(ipi.equals("0")){
				ipi = "00";
			}
			endString += ipi;
			System.out.println(ipi);
			
		}
		endString = "0f" + endString + "0000";
		System.out.println(endString);
		Log.i("App_Tcp==", "endString=" + endString);
	
	}
 
== endString==
 0fa95fcea80035f424000003f310f0540000000000000000000000000000000000
 
通信
 
由于项目要求实时性比较高，所以采取App直接连接硬件的方案
 采用Socket直接连接IP和端口号
 实现方法：
 安卓Socket的使用

沿墙走
 
项目内容：机器人沿着墙壁行走（采用左手规则或者右手规则）。
 相关模块：启动电机、延时等待、停止电机、红外测障、碰撞检测、直行、转向、永远循环。
 
程序分析
 沿墙走，顾名思义，即机器人靠近墙壁行走。采用左手规则时，墙壁在机器人的左边；而采用右手规则时，墙壁在机器人的右边。下面以左手规则为例加以说明。
 
 
如图所示，要实现沿墙走，机器人可以采取划弧线的方法，边前进边检测墙壁。机器人如果发现前方有障碍物，说明正对着墙壁了，就右转约90度；如果发现左方有障碍物，说明机器人左侧对着墙壁了，就右转约60度；否则就划弧线前进。有时候机器人会撞上墙壁，为此机器人还需具有处理碰撞的功能。在程序中，我们让机器人一旦检测到碰撞，就后退一点，并右转约60度。沿墙走的例程如下图所示。
 
 
参数设置
 “红外测障”模块：检测完成后，进行条件判断，条件表达式为
 红外变量一 ＝＝ 左
 “条件判断”模块：条件表达式为
 红外变量一 ＝＝ 前
 “启动电机”模块：左电机功率20，右电机功率30。
 “延时等待”模块：时间0.05秒。
 “碰撞检测”模块：条件表达式为
 碰撞变量！＝无
 其余模块的参数自行设置，并需反复调整，直至满意为止。
 仿真运行
 仿真运行时，加载单房间场地、标准灭火场地、走迷宫场地均可。读者也可以自己创建一个场地，让机器人在其中运行。