JSGT/JST_RC_GROUND_STATION.ino at main · doubledev-dev/JSGT · 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
#include <WiFi.h>
#include <esp_now.h>

typedef struct
{
    int16_t throttle; // -1000 .. +1000
    int16_t steering; // -1000 .. +1000
} ControlPacket;

ControlPacket pkt;

// broadcast
uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

// timing
const uint32_t TX_INTERVAL_MS = 50;
uint32_t lastTxTime = 0;

int16_t mapByteSteer(int v)
{
    // v: 0..255  →  -1000..+1000
    v = constrain(v, 0, 255);
    return map(v, 0, 255, 75, 125);
}

int16_t mapByteThrot(int v)
{
    v = constrain(v, 0, 255);
    return map(v, 0, 255, 1500, 1750);
}

void setup()
{
    Serial.begin(115200);

    WiFi.mode(WIFI_STA);
    if (esp_now_init() != ESP_OK)
    {
        Serial.println("ESP-NOW init failed");
        while (1)
            ;
    }

    esp_now_peer_info_t peer = {};
    memcpy(peer.peer_addr, broadcastAddr, 6);
    peer.channel = 0;
    peer.encrypt = false;
    esp_now_add_peer(&peer);

    Serial.println("JOY ready. Send <steering,throttle>");
}

void loop()
{
    if (Serial.available())
    {
        String cmd = Serial.readStringUntil('\n');
        cmd.trim();

        if (cmd.startsWith("<") && cmd.endsWith(">"))
        {
            cmd = cmd.substring(1, cmd.length() - 1);

            int commaIdx = cmd.indexOf(',');
            if (commaIdx > 0)
            {
                int steeringRaw = cmd.substring(0, commaIdx).toInt();
                int throttleRaw = cmd.substring(commaIdx + 1).toInt();

                pkt.steering = mapByteSteer(steeringRaw);
                pkt.throttle = mapByteThrot(throttleRaw);

                // esp_now_send(
                //   broadcastAddr,
                //   (uint8_t*)&pkt,
                //   sizeof(pkt)
                // );
            }
        }
    }
    /* ---------- 2) ส่ง ESP-NOW ทุก ๆ 50 ms ---------- */
    if (millis() - lastTxTime >= TX_INTERVAL_MS)
    {
        lastTxTime = millis();

        esp_now_send(
            broadcastAddr,
            (uint8_t *)&pkt,
            sizeof(pkt));

        // Serial.printf(
        //   "TX steer:%d thr:%d\n",
        //   pkt.steering,
        //   pkt.throttle
        // );
    }
}