ADAS/ADAS.cpp at main · OmShirse/ADAS · 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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
#include<ets_sys.h>
#include<osapi.h>
#include<gpio.h>
#include<uart.h>

#define GPIO_SET_REG 0x60000304
#define GPIO_CLR_REG 0x60000308

#define GPIO_SET(pin) (*(volatile uint32_t*)GPIO_SET_REG = (1 << pin))
#define GPIO_CLR(pin) (*(volatile uint32_t*)GPIO_CLR_REG = (1 << pin))

//  MOTOR PINS

// Front Right
#define M1_IN1 5
#define M1_IN2 4

// Front Left
#define M2_IN1 12
#define M2_IN2 13

// Rear Left
#define M3_IN1 14
#define M3_IN2 16

// Rear Right
#define M4_IN1 2
#define M4_IN2 15


uint8_t command = 0;


// LOW LEVEL MOTOR CONTROL

static inline void motor_forward(uint8_t a,uint8_t b)
{
    GPIO_SET(a);
    GPIO_CLR(b);
}

static inline void motor_backward(uint8_t a,uint8_t b)
{
    GPIO_CLR(a);
    GPIO_SET(b);
}

static inline void motor_stop(uint8_t a,uint8_t b)
{
    GPIO_CLR(a);
    GPIO_CLR(b);
}


// ===== MOVEMENT FUNCTIONS =====

void stop_all()
{
    motor_stop(M1_IN1,M1_IN2);
    motor_stop(M2_IN1,M2_IN2);
    motor_stop(M3_IN1,M3_IN2);
    motor_stop(M4_IN1,M4_IN2);
}

void move_forward()
{
    motor_forward(M1_IN1,M1_IN2);
    motor_forward(M2_IN1,M2_IN2);
    motor_forward(M3_IN1,M3_IN2);
    motor_forward(M4_IN1,M4_IN2);
}

void move_backward()
{
    motor_backward(M1_IN1,M1_IN2);
    motor_backward(M2_IN1,M2_IN2);
    motor_backward(M3_IN1,M3_IN2);
    motor_backward(M4_IN1,M4_IN2);
}

void rotate_left()
{
    motor_backward(M1_IN1,M1_IN2);
    motor_forward(M2_IN1,M2_IN2);
    motor_backward(M3_IN1,M3_IN2);
    motor_forward(M4_IN1,M4_IN2);
}

void rotate_right()
{
    motor_forward(M1_IN1,M1_IN2);
    motor_backward(M2_IN1,M2_IN2);
    motor_forward(M3_IN1,M3_IN2);
    motor_backward(M4_IN1,M4_IN2);
}


//  UART RECEIVE HANDLER

void ICACHE_FLASH_ATTR uart_rx_handler(void *arg)
{
    uint8_t data;

    while(UART_RXFIFO_CNT(0))
    {
        data = READ_PERI_REG(UART_FIFO(0)) & 0xFF;

        if(data < 15)
            command = data;
    }
}


// SYSTEM INIT

void ICACHE_FLASH_ATTR user_init(void)
{
    uart_init(BIT_RATE_9600,BIT_RATE_9600);

    PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO5_U,FUNC_GPIO5);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO4_U,FUNC_GPIO4);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U,FUNC_GPIO12);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U,FUNC_GPIO13);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U,FUNC_GPIO14);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO2_U,FUNC_GPIO2);

    gpio_output_set(0,0,
        (1<<M1_IN1)|(1<<M1_IN2)|
        (1<<M2_IN1)|(1<<M2_IN2)|
        (1<<M3_IN1)|(1<<M3_IN2)|
        (1<<M4_IN1)|(1<<M4_IN2),
        0);

    ETS_UART_INTR_ATTACH(uart_rx_handler,NULL);
    ETS_UART_INTR_ENABLE();

    os_printf("ADAS READY\n");
}


// MAIN TASK LOOP

void ICACHE_FLASH_ATTR user_loop()
{
    switch(command)
    {
        case 0: stop_all(); break;
        case 2: move_forward(); break;
        case 7: move_backward(); break;
        case 9: rotate_left(); break;
        case 10: rotate_right(); break;
    }
}