ArduinoRoboticVacuumCleaner/RoombaProperties.cpp at master · ErrorJan/ArduinoRoboticVacuumCleaner · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#include "RoombaProperties.hpp"

#include "Vec2.hpp"
#include <Arduino.h>

Vec2 Direction2Vector( RoombaDirection dir )
{
    switch ( dir )
    {
        case RoombaDirection::NORTH:
            return Vec2( 0, 1 );
        case RoombaDirection::EAST:
            return Vec2( 1, 0 );
        case RoombaDirection::SOUTH:
            return Vec2( 0, -1 );
        case RoombaDirection::WEST:
            return Vec2( -1, 0 );
        default:
            return Vec2( 0, 0 );
    }
}

RoombaDirection Vector2Direction( Vec2 dir )
{
    if ( dir == Vec2( 0, 1 ) )
        return RoombaDirection::NORTH;
    else if ( dir == Vec2( 1, 0 ) )
        return RoombaDirection::EAST;
    else if ( dir == Vec2( 0, -1 ) )
        return RoombaDirection::SOUTH;
    else if ( dir == Vec2( -1, 0 ) )
        return RoombaDirection::WEST;
    else
        return RoombaDirection::NODIRECTION;
}

RoombaDirection RightSideDirection( RoombaDirection currentDirection )
{
    Vec2 currentDirectionVec = Direction2Vector( currentDirection );
    Vec2 rightSideVec = Vec2( currentDirectionVec.y, currentDirectionVec.x * -1 );
    return Vector2Direction( rightSideVec );
}

RoombaDirection LeftSideDirection( RoombaDirection currentDirection )
{
    Vec2 currentDirectionVec = Direction2Vector( currentDirection );
    Vec2 rightSideVec = Vec2( currentDirectionVec.y * -1, currentDirectionVec.x );
    return Vector2Direction( rightSideVec );
}

void PrintDirection( RoombaDirection dir )
{
    switch ( dir )
    {
        case RoombaDirection::NORTH:
            Serial.println( F("North") );
            break;
        case RoombaDirection::EAST:
            Serial.println( F("East") );
            break;
        case RoombaDirection::SOUTH:
            Serial.println( F("South") );
            break;
        case RoombaDirection::WEST:
            Serial.println( F("West") );
            break;
        default:
            Serial.println( F("Unknown Direction") );
    }
}

Vec2 RealPos2GridPos( Vec2 pos, float gridSize )
{
    Vec2 roundedPos;

    roundedPos.x = round( pos.x / gridSize );
    roundedPos.y = round( pos.y / gridSize );

    return roundedPos;
}