mtkernel_3_microbit/app_sample/Drive.c at main · grace2riku/mtkernel_3_microbit · 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
81
82
83
84
85
86
#include "Motor.h"

#include "Drive.h"

/* 前回進行方向 */
static directionVector_t lastDirection;

static FrontBack_t dr_cvtFrontBack(directForward_t);
static LeftRight_t dr_cvtLeftRight(directTurn_t);

void dr_init(void) {
    lastDirection.forward = eStopForward;
    lastDirection.turn = eStraight;
    return;
}


void dr_term(void) {
    return;
}


void dr_move(directionVector_t direct) {
    mt_direction_t motor;   /* モータの駆動方向 */

    if ((direct.forward != lastDirection.forward)
        || (direct.turn != lastDirection.turn)) {
        /* 前後方向または左右方向が変化した時にモータを駆動 */

        /* 前後方向値を変換する */
        motor.front_back = dr_cvtFrontBack(direct.forward);

        /* 左右方向値を変換する */
        motor.left_right = dr_cvtLeftRight(direct.turn);

        lastDirection = direct;

        /* モータを駆動する */
        mt_drvMotor(motor);
    }

    return;
}

static FrontBack_t dr_cvtFrontBack(directForward_t forward) {
    FrontBack_t front_back;

    switch (forward) {
    case eStopForward:
        front_back = eSTOP;
        break;
    case eMoveForward:
        front_back = eFRONT;
        break;
    case eMoveBackward:
    	front_back = eBACK;
    	break;
    default:
        /* 停止させる */
        front_back = eSTOP;
        break;
    }
    return front_back;
}

static LeftRight_t dr_cvtLeftRight(directTurn_t turn) {
    LeftRight_t left_right; /* 左右方向 */

    switch (turn)
    {
    case eTurnLeft:
        left_right = eLEFT_TURN;
        break;
    case eStraight:
        left_right = eNO_TURN;
        break;
    case eTurnRight:
        left_right = eRIGHT_TURN;
        break;
    default:
        /* 左右方向なし */
        left_right = eNO_TURN;
        break;
    }
    return left_right;
}