两个点已知一个点的方向,判断另一个点在这个的方位(左前/右后)

使用条件:已知两个点的经纬度,以及一个点(这个点为主点,另一个点为副点)的方向(是一个在原始坐标系上的角度),该方向是由定位自动获取的;

备注:原始坐标系:以正北为y轴正方向, 其中经度为x轴,纬度为y轴

   主点坐标系:以主点的方向为y轴正方向,其中以主点的方向所在的直线为y轴,过主点且与y轴垂直的直线为x轴,

第一步:求出副点在原始坐标系上的方向,

//根据两个点的经纬度,以一个点(userLocationCoordinate)为坐标中心,y轴正方向为正北方向(即为 0°),求出另一个点(rodeLocationCoordinate)关于正北方向的角度(0° ~ 360°)
- (double)judgeAngle:(CLLocationCoordinate2D)userLocationCoordinate andRoadInfoCoordinate:(CLLocationCoordinate2D)rodeLocationCoordinate {
    double a = rodeLocationCoordinate.longitude - userLocationCoordinate.longitude;  //经度差
    double b = rodeLocationCoordinate.latitude - userLocationCoordinate.latitude;    //纬度差
    double c = hypot(fabs(a), fabs(b));
    double cosy = 0.0;
    double angle = 0;
    
    if (a > 0 && b > 0) {                  // 判断road点在user点的东北位置
        cosy = b / c;
        angle = 0;
    }else if (a == 0 && b > 0) {           //在正北位置
        angle = -90;
    }else if (a > 0 && b < 0) {            // 判断road点在user点的东南位置
        cosy = a / c;
        angle = 90;
    }else if (a > 0 && b == 0) {           //在正东位置
        angle = 90;
    }else if (a < 0 && b < 0) {            // 判断road点在user点的西南位置
        cosy = fabs(b) / c;
        angle = 180;
    }else if (a == 0 && b < 0) {           //在正南位置
        angle = 90;
    }else if (a < 0 && b > 0) {            // 判断road点在user点的西北位置
        cosy = fabs(a) / c;
        angle = 270;
    }else if (a < 0 && b == 0) {           //在正西位置
        angle = 180;
    }
    
    double m = acos(cosy);
    //n 即以正北为 0 的总角度
    double n = ( m / M_PI ) * 180 + angle;
    
    return n;
}

第二步:先将副点的坐标系从原始坐标系转换到主点坐标系。下面代码中返回值即是相应的方位

原理:从两个点在原始坐标系的方向(角度)来判断,当副点的原方向小于主点的原方向时,副点在主点坐标系上的方向为 “副点原方向 + 360° - 主点原方向”;当副点的原方向小于主点的原方向时,副点在主点坐标系的方向为 “副点原方向 - 主点原方向”,以此来求出副点主方向,当副点主方向大于0小于90时,副点在主点的右前方,当副点主方向等于90时,副点在主点的正右方,以此类推,这样就能判断出副点在主点的方位了

备注:副点原方向:第一步中求出的 副点在原始坐标系中的方向;

  主点原方向:已知条件中的主点基于原始坐标系的方向;

  副点主坐标:副点基于主点坐标系中的方向;

//根据两个点经纬度和主点(userLocationCoordinate)的角度,判断出另一个点(rodeLocationCoordinate)在主点的方位
- (int)judgeDirection:(CLLocationCoordinate2D)userLocationCoordinate andRoadInfo:(CLLocationCoordinate2D)rodeLocationCoordinate andUserAngle:(double)userAngle{
    double roadAngle = [self judgeAngle:userLocationCoordinate andRoadInfoCoordinate:rodeLocationCoordinate];
    double angleGap = roadAngle - userAngle;
    
    int direction = 0;
    
    if (angleGap < 0) {
        angleGap = roadAngle + 360 - userAngle;
    }else {
        angleGap = roadAngle - userAngle;
    }
    
    if (angleGap > 0 && angleGap < 90) {                  // road点在user点的右前方
        direction = 1;
    }else if(angleGap == 90){                             // road点在user点的正右方
        direction = 2;
    }else if (angleGap > 90 && angleGap < 180) {          // road点在user点的右后方
        direction = 3;
    }else if(angleGap == 180){                            // road点在user点的正后方
        direction = 4;
    }else if (angleGap > 180 && angleGap < 270) {         // road点在user点的左后方
        direction = 5;
    }else if(angleGap == 270){                            // road点在user点的正左方
        direction = 6;
    }else if (angleGap > 270 && angleGap < 360) {         // road点在user点的左前方
        direction = 7;
    }else if(angleGap == 0){                              // road点在user点的正前方
        direction = 0;
    }
    return direction;
}




好的,我能为您提供帮助。以下是一个基本的4路红外循迹小车代码: ``` #include <AFMotor.h> // 加入驱动电机用到的库 AF_DCMotor motor1(1); // 定义电机1初始化 AF_DCMotor motor2(2); // 定义电机2初始化 AF_DCMotor motor3(3); // 定义电机3初始化 AF_DCMotor motor4(4); // 定义电机4初始化 int in1 = 2; // 定义IR1输入引脚 int in2 = 3; // 定义IR2输入引脚 int in3 = 4; // 定义IR3输入引脚 int in4 = 5; // 定义IR4输入引脚 void setup() { pinMode(in1, INPUT); // 设置IR1输入引脚为输入模式 pinMode(in2, INPUT); // 设置IR2输入引脚为输入模式 pinMode(in3, INPUT); // 设置IR3输入引脚为输入模式 pinMode(in4, INPUT); // 设置IR4输入引脚为输入模式 Serial.begin(9600); // 初始化串行通信 } void loop() { int IR1 = digitalRead(in1); // 读取IR1输入状态 int IR2 = digitalRead(in2); // 读取IR2输入状态 int IR3 = digitalRead(in3); // 读取IR3输入状态 int IR4 = digitalRead(in4); // 读取IR4输入状态 Serial.print("IR1:"); // 打印IR1状态 Serial.print(IR1); Serial.print("IR2:"); // 打印IR2状态 Serial.print(IR2); Serial.print("IR3:"); // 打印IR3状态 Serial.print(IR3); Serial.print("IR4:"); // 打印IR4状态 Serial.println(IR4); // 根据传感器状态控制小车行走 if ((IR1 == 0) && (IR2 == 1) && (IR3 == 1) && (IR4 == 0)) { motor1.setSpeed(255); motor1.run(FORWARD); motor2.setSpeed(255); motor2.run(BACKWARD); motor3.setSpeed(255); motor3.run(FORWARD); motor4.setSpeed(255); motor4.run(BACKWARD); } else if ((IR1 == 0) && (IR2 == 1) && (IR3 == 0) && (IR4 == 0)) { motor1.setSpeed(255); motor1.run(FORWARD); motor2.setSpeed(255); motor2.run(BACKWARD); motor3.setSpeed(255); motor3.run(BACKWARD); motor4.setSpeed(255); motor4.run(FORWARD); } else if ((IR1 == 0) && (IR2 == 0) && (IR3 == 1) && (IR4 == 0)) { motor1.setSpeed(255); motor1.run(BACKWARD); motor2.setSpeed(255); motor2.run(FORWARD); motor3.setSpeed(255); motor3.run(FORWARD); motor4.setSpeed(255); motor4.run(BACKWARD); } else if ((IR1 == 1) && (IR2 == 0) && (IR3 == 1) && (IR4 == 0)) { motor1.setSpeed(255); motor1.run(FORWARD); motor2.setSpeed(255); motor2.run(BACKWARD); motor3.setSpeed(255); motor3.run(BACKWARD); motor4.setSpeed(255); motor4.run(FORWARD); } else { motor1.setSpeed(0); motor1.run(RELEASE); motor2.setSpeed(0); motor2.run(RELEASE); motor3.setSpeed(0); motor3.run(RELEASE); motor4.setSpeed(0); motor4.run(RELEASE); } } ``` 代码的基本思路是,在一个死循环中不断读取4个红外传感器的输入状态,然后根据红外传感器的状态控制小车的运动。如果左前和右前红外传感器都检测到黑色线路,那么小车就朝前行走;如果左前、左后和右前、右后两个对角线红外传感器都检测到黑色线路,那么小车就会原地掉头;如果左前、左后和右前都检测到黑色线路,那么小车就会往右转弯;如果右前、右后左前都检测到黑色线路,那么小车就会往左转弯。如果没有检测到黑色线路,那么小车就停止。 请注意,以上代码仅供参考,您需要根据实际情况设计和调整代码。此外,您需要检查和确定驱动电机的适当设置和接线。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值