ME 461 Final Project: Robot Dog

#include <Bluepad32.h>
#include <HardwareSerial.h> // GR: This is for Serial sending to TI C2000

ControllerPtr myControllers[BP32_MAX_GAMEPADS];

// GR: This is for Serial sending to TI C2000
HardwareSerial SerialToC2000(2);  // use UART2

// GR: function to send ps5 data
static void send_ps5_state(int16_t lx, int16_t ly, int16_t rx, int16_t ry, int16_t br, int16_t th, uint16_t btn)
{
  // headers for state machine
  const uint8_t header[2] = {0xAA, 0x55}; // Start bit
  // packet will be 12 bytes
  int16_t joystick[6] = {0};
  uint16_t buttons[1] = {0};
  // lx (2 bytes)
  joystick[0] = lx;
  // ly (2 bytes)
  joystick[1] = ly;
  // rx (2 bytes)
  joystick[2] = rx;
  // ry (2 bytes)
  joystick[3] = ry;
  // br (2 bytes)
  joystick[4] = br;
  // th (2 bytes)
  joystick[5] = th;
  // btn (2 bytes)
  buttons[0] = btn; // bit mask

  SerialToC2000.write(header[0]); // send start bit
  SerialToC2000.write(header[1]); // send second bit
  SerialToC2000.write((uint8_t*)joystick, sizeof(joystick)); // send joystick data
  SerialToC2000.write((uint8_t*)buttons, sizeof(buttons)); // send controller button data
}

// This callback gets called any time a new gamepad is connected.
// Up to 4 gamepads can be connected at the same time.
void onConnectedController(ControllerPtr ctl) {
    bool foundEmptySlot = false;
    for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
        if (myControllers[i] == nullptr) {
            Serial.printf("CALLBACK: Controller is connected, index=%d\n", i);
            // Additionally, you can get certain gamepad properties like:
            // Model, VID, PID, BTAddr, flags, etc.
            ControllerProperties properties = ctl->getProperties();
            Serial.printf("Controller model: %s, VID=0x%04x, PID=0x%04x\n", ctl->getModelName().c_str(), properties.vendor_id,
                           properties.product_id);
            myControllers[i] = ctl;
            foundEmptySlot = true;
            break;
        }
    }
    if (!foundEmptySlot) {
        Serial.println("CALLBACK: Controller connected, but could not found empty slot");
    }
}

void onDisconnectedController(ControllerPtr ctl) {
    bool foundController = false;

    for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
        if (myControllers[i] == ctl) {
            Serial.printf("CALLBACK: Controller disconnected from index=%d\n", i);
            myControllers[i] = nullptr;
            foundController = true;
            break;
        }
    }

    if (!foundController) {
        Serial.println("CALLBACK: Controller disconnected, but not found in myControllers");
    }
}

void dumpGamepad(ControllerPtr ctl) {
    Serial.printf(
        "idx=%d, dpad: 0x%02x, buttons: 0x%04x, axis L: %4d, %4d, axis R: %4d, %4d, brake: %4d, throttle: %4d, "
        "misc: 0x%02x, gyro x:%6d y:%6d z:%6d, accel x:%6d y:%6d z:%6d\n",
        ctl->index(),        // Controller Index
        ctl->dpad(),         // D-pad
        ctl->buttons(),      // bitmask of pressed buttons
        ctl->axisX(),        // (-511 - 512) left X Axis
        ctl->axisY(),        // (-511 - 512) left Y axis
        ctl->axisRX(),       // (-511 - 512) right X axis
        ctl->axisRY(),       // (-511 - 512) right Y axis
        ctl->brake(),        // (0 - 1023): brake button
        ctl->throttle(),     // (0 - 1023): throttle (AKA gas) button
        ctl->miscButtons(),  // bitmask of pressed "misc" buttons
        ctl->gyroX(),        // Gyro X
        ctl->gyroY(),        // Gyro Y
        ctl->gyroZ(),        // Gyro Z
        ctl->accelX(),       // Accelerometer X
        ctl->accelY(),       // Accelerometer Y
        ctl->accelZ()        // Accelerometer Z
    );
}

void dumpMouse(ControllerPtr ctl) {
    Serial.printf("idx=%d, buttons: 0x%04x, scrollWheel=0x%04x, delta X: %4d, delta Y: %4d\n",
                   ctl->index(),        // Controller Index
                   ctl->buttons(),      // bitmask of pressed buttons
                   ctl->scrollWheel(),  // Scroll Wheel
                   ctl->deltaX(),       // (-511 - 512) left X Axis
                   ctl->deltaY()        // (-511 - 512) left Y axis
    );
}

void dumpKeyboard(ControllerPtr ctl) {
    static const char* key_names[] = {
        // clang-format off
        // To avoid having too much noise in this file, only a few keys are mapped to strings.
        // Starts with "A", which is offset 4.
        "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V",
        "W", "X", "Y", "Z", "1", "2", "3", "4", "5", "6", "7", "8", "9", "0",
        // Special keys
        "Enter", "Escape", "Backspace", "Tab", "Spacebar", "Underscore", "Equal", "OpenBracket", "CloseBracket",
        "Backslash", "Tilde", "SemiColon", "Quote", "GraveAccent", "Comma", "Dot", "Slash", "CapsLock",
        // Function keys
        "F1", "F2", "F3", "F4", "F5", "F6", "F7", "F8", "F9", "F10", "F11", "F12",
        // Cursors and others
        "PrintScreen", "ScrollLock", "Pause", "Insert", "Home", "PageUp", "Delete", "End", "PageDown",
        "RightArrow", "LeftArrow", "DownArrow", "UpArrow",
        // clang-format on
    };
    static const char* modifier_names[] = {
        // clang-format off
        // From 0xe0 to 0xe7
        "Left Control", "Left Shift", "Left Alt", "Left Meta",
        "Right Control", "Right Shift", "Right Alt", "Right Meta",
        // clang-format on
    };
    Serial.printf("idx=%d, Pressed keys: ", ctl->index());
    for (int key = Keyboard_A; key <= Keyboard_UpArrow; key++) {
        if (ctl->isKeyPressed(static_cast<KeyboardKey>(key))) {
            const char* keyName = key_names[key-4];
            Serial.printf("%s,", keyName);
       }
    }
    for (int key = Keyboard_LeftControl; key <= Keyboard_RightMeta; key++) {
        if (ctl->isKeyPressed(static_cast<KeyboardKey>(key))) {
            const char* keyName = modifier_names[key-0xe0];
            Serial.printf("%s,", keyName);
        }
    }
    Console.printf("\n");
}

void dumpBalanceBoard(ControllerPtr ctl) {
    Serial.printf("idx=%d,  TL=%u, TR=%u, BL=%u, BR=%u, temperature=%d\n",
                   ctl->index(),        // Controller Index
                   ctl->topLeft(),      // top-left scale
                   ctl->topRight(),     // top-right scale
                   ctl->bottomLeft(),   // bottom-left scale
                   ctl->bottomRight(),  // bottom-right scale
                   ctl->temperature()   // temperature: used to adjust the scale value's precision
    );
}

void processGamepad(ControllerPtr ctl) {
    // There are different ways to query whether a button is pressed.
    // By query each button individually:
    //  a(), b(), x(), y(), l1(), etc...
    if (ctl->a()) {
        static int colorIdx = 0;
        // Some gamepads like DS4 and DualSense support changing the color LED.
        // It is possible to change it by calling:
        switch (colorIdx % 3) {
            case 0:
                // Red
                ctl->setColorLED(255, 0, 0);
                break;
            case 1:
                // Green
                ctl->setColorLED(0, 255, 0);
                break;
            case 2:
                // Blue
                ctl->setColorLED(0, 0, 255);
                break;
        }
        colorIdx++;
    }

    if (ctl->b()) {
        // Turn on the 4 LED. Each bit represents one LED.
        static int led = 0;
        led++;
        // Some gamepads like the DS3, DualSense, Nintendo Wii, Nintendo Switch
        // support changing the "Player LEDs": those 4 LEDs that usually indicate
        // the "gamepad seat".
        // It is possible to change them by calling:
        ctl->setPlayerLEDs(led & 0x0f);
    }

    if (ctl->x()) {
        // Some gamepads like DS3, DS4, DualSense, Switch, Xbox One S, Stadia support rumble.
        // It is possible to set it by calling:
        // Some controllers have two motors: "strong motor", "weak motor".
        // It is possible to control them independently.
        ctl->playDualRumble(0 /* delayedStartMs */, 250 /* durationMs */, 0x80 /* weakMagnitude */,
                            0x40 /* strongMagnitude */);
    }

    // Another way to query controller data is by getting the buttons() function.
    // See how the different "dump*" functions dump the Controller info.
    dumpGamepad(ctl);

    // Basic fields we care about
    int16_t lx  = ctl->axisX();     // Left stick X (-511..512)
    int16_t ly  = ctl->axisY();     // Left stick Y (-511..512)
    int16_t rx  = ctl->axisRX();     // Right stick X (-511..512)
    int16_t ry  = ctl->axisRY();     // Right stick Y (-511..512)
    int16_t br  = ctl->brake();     // 0..1023
    int16_t th  = ctl->throttle();  // 0..1023
    uint16_t btn = ctl->buttons();  // bitmask


    // Send a simple CSV line starting with 'J' and ending with '\n'
    send_ps5_state(lx, ly, rx, ry, br, th, btn);
    
    // SerialToC2000.printf("J,%d,%d,%d,%d,%u\n", ax, ay, br, th, btn);
}

void processMouse(ControllerPtr ctl) {
    // This is just an example.
    if (ctl->scrollWheel() > 0) {
        // Do Something
    } else if (ctl->scrollWheel() < 0) {
        // Do something else
    }

    // See "dumpMouse" for possible things to query.
    dumpMouse(ctl);
}

void processKeyboard(ControllerPtr ctl) {
    if (!ctl->isAnyKeyPressed())
        return;

    // This is just an example.
    if (ctl->isKeyPressed(Keyboard_A)) {
        // Do Something
        Serial.println("Key 'A' pressed");
    }

    // Don't do "else" here.
    // Multiple keys can be pressed at the same time.
    if (ctl->isKeyPressed(Keyboard_LeftShift)) {
        // Do something else
        Serial.println("Key 'LEFT SHIFT' pressed");
    }

    // Don't do "else" here.
    // Multiple keys can be pressed at the same time.
    if (ctl->isKeyPressed(Keyboard_LeftArrow)) {
        // Do something else
        Serial.println("Key 'Left Arrow' pressed");
    }

    // See "dumpKeyboard" for possible things to query.
    dumpKeyboard(ctl);
}

void processBalanceBoard(ControllerPtr ctl) {
    // This is just an example.
    if (ctl->topLeft() > 10000) {
        // Do Something
    }

    // See "dumpBalanceBoard" for possible things to query.
    dumpBalanceBoard(ctl);
}

void processControllers() {
    for (auto myController : myControllers) {
        if (myController && myController->isConnected() && myController->hasData()) {
            if (myController->isGamepad()) {
                processGamepad(myController);
            } else if (myController->isMouse()) {
                processMouse(myController);
            } else if (myController->isKeyboard()) {
                processKeyboard(myController);
            } else if (myController->isBalanceBoard()) {
                processBalanceBoard(myController);
            } else {
                Serial.println("Unsupported controller");
            }
        }
    }
}

// Arduino setup function. Runs in CPU 1
void setup() {
    Serial.begin(115200);
    Serial.printf("Firmware: %s\n", BP32.firmwareVersion());
    const uint8_t* addr = BP32.localBdAddress();
    Serial.printf("BD Addr: %2X:%2X:%2X:%2X:%2X:%2X\n", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);

    // UART to C2000: RX on GPIO13 (optional), TX on GPIO15
    SerialToC2000.begin(115200, SERIAL_8N1, 13 /*rx*/, 15 /*tx*/);

    // Setup the Bluepad32 callbacks
    BP32.setup(&onConnectedController, &onDisconnectedController);

    // "forgetBluetoothKeys()" should be called when the user performs
    // a "device factory reset", or similar.
    // Calling "forgetBluetoothKeys" in setup() just as an example.
    // Forgetting Bluetooth keys prevents "paired" gamepads to reconnect.
    // But it might also fix some connection / re-connection issues.
    BP32.forgetBluetoothKeys();

    // Enables mouse / touchpad support for gamepads that support them.
    // When enabled, controllers like DualSense and DualShock4 generate two connected devices:
    // - First one: the gamepad
    // - Second one, which is a "virtual device", is a mouse.
    // By default, it is disabled.
    BP32.enableVirtualDevice(false);
}

// Arduino loop function. Runs in CPU 1.
void loop() {
    // This call fetches all the controllers' data.
    // Call this function in your main loop.
    bool dataUpdated = BP32.update();
    if (dataUpdated)
        processControllers();

    // The main loop must have some kind of "yield to lower priority task" event.
    // Otherwise, the watchdog will get triggered.
    // If your main loop doesn't have one, just add a simple `vTaskDelay(1)`.
    // Detailed info here:
    // https://stackoverflow.com/questions/66278271/task-watchdog-got-triggered-the-tasks-did-not-reset-the-watchdog-in-time

    //     vTaskDelay(1);
    delay(150);
}

// ...

typedef struct {
    int16_t lx;
    int16_t ly;
    int16_t rx;
    int16_t ry;
    uint16_t brake;
    uint16_t throttle;
    uint16_t button;
    // None pressed = 0;
    // X => 1
    // O => 2
    // square => 4
    // triangle => 8
} tDataFromESP32;

// ...

// for SerialC
#ifdef _FLASH
#pragma CODE_SECTION(RXCINT_recv_ready, ".TI.ramfunc");
#endif
__interrupt void RXCINT_recv_ready(void)
{
    RXCdata = ScicRegs.SCIRXBUF.all;

    /* SCI PE or FE error */
    if (RXCdata & 0xC000) {
        ScicRegs.SCICTL1.bit.SWRESET = 0;
        ScicRegs.SCICTL1.bit.SWRESET = 1;
        ScicRegs.SCIFFRX.bit.RXFIFORESET = 0;
        ScicRegs.SCIFFRX.bit.RXFIFORESET = 1;
    } else {
        RXCdata = RXCdata & 0x00FF;
        numRXC ++;
    }

    // GR: Final Project Joystick
    if(COMC_state == 0){
        // GR: RXCdata is the first char of our start bits
        if(RXCdata == 0xAA)
        {
            COMC_state = 1;
        }
        else
        {
            COMC_state = 0;
        }
    }

    else if(COMC_state == 1)
    {
        // GR: After first start bit, we check for the second
        if(RXCdata == 0x55)
        {
            COMC_state = 10; // into read mode of serial data
            received_COMC_count_int = 0;
        }
        else
        {
            COMC_state = 0;
        }
    }

    else if(COMC_state == 10)
    {
//        if(received_COMC_count_int < 2){
        RX_data_in_joystick[received_COMC_count_int] = RXCdata & 0x00FF; // GR: type cast the data as it comes in
        received_COMC_count_int++;
//        }
//        else if(received_COMC_count_int >=2 && received_COMC_count_uint < 3)
//        {
//            RX_data_in_button[received_COMC_count_uint] = RXCdata;
//            received_COMC_count_uint++;
//        }
    }
    if(received_COMC_count_int >= 14)
    {
        received_COMC_count_int = 0;
        //        received_COMC_count_uint = 0;
        COMC_state = 0;
        // GR: little-endian: low byte first, then high byte
        lx =  (int16_t)((RX_data_in_joystick[1] << 8) | (RX_data_in_joystick[0] & 0x00FF));
        ly =  (int16_t)((RX_data_in_joystick[3] << 8) | (RX_data_in_joystick[2] & 0x00FF));
        rx =  (int16_t)((RX_data_in_joystick[5] << 8) | (RX_data_in_joystick[4] & 0x00FF));
        ry =  (int16_t)((RX_data_in_joystick[7] << 8) | (RX_data_in_joystick[6] & 0x00FF));

        br = (uint16_t)((RX_data_in_joystick[9] << 8) | (RX_data_in_joystick[8] & 0x00FF));
        th = (uint16_t)((RX_data_in_joystick[11] << 8) | (RX_data_in_joystick[10] & 0x00FF));
        bt = (uint16_t)((RX_data_in_joystick[13] << 8) | (RX_data_in_joystick[12] & 0x00FF));


        // GR: store in struct
        DataFromESP32.lx       = lx;
        DataFromESP32.ly       = ly;
        DataFromESP32.rx       = rx;
        DataFromESP32.ry       = ry;
        DataFromESP32.brake    = br;
        DataFromESP32.throttle = th;
        DataFromESP32.button   = bt;

        NewCOMCdata = 1;
    }

    ScicRegs.SCIFFRX.bit.RXFFINTCLR = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP8;
}

// ...

phiR = (rWH/wR) * (RightWheel - LeftWheel);

  thetaDotR = (RightWheel - RightWheel_1) / 0.004;
  thetaDotL = (LeftWheel - LeftWheel_1) / 0.004;

  thetaAvg = 0.5 * (RightWheel + LeftWheel);
  thetaDotAvg = 0.5 * (thetaDotR + thetaDotL);

  xDotR = rWH*thetaDotAvg*cos(phiR);
  yDotR = rWH*thetaDotAvg*sin(phiR);

  xOdo = xOdo + xDotR*0.004;
  yOdo = yOdo + yDotR*0.004;

  // GR: IMU based calculation
  ax_body = accelx * g;
  ay_body = accely * g;
  az_body = accelz * g;

  // GR: Horizontal motion, x-z plane for IMU mounted orientation
  float a_fwd_body = -az_body - g * sinf(tilt_value);
  float a_left_body = -ax_body;

  // GR: Rotate body
  float cosY = cosf(phiR);
  float sinY = sinf(phiR);

  float ax_world = a_fwd_body * cosY - a_left_body * sinY;
  float ay_world = a_fwd_body * sinY + a_left_body * cosY;

  // GR: Integrate acceleration
  vxIMU += ax_world * dt;
  vyIMU += ay_world * dt;

  // GR: Integrate velocity
  xIMU += vxIMU * dt;
  yIMU += vyIMU * dt;

  // GR: Kalman Fusion (IMU prediction + Odometry measurement)
  // GR: X-axis Kalman Filter
  float x_pred = xR + vxIMU * dt;
  float P_x_pred = P_x + Q_x;

  float innov_x = xOdo - x_pred;
  float K_x = P_x_pred/(P_x_pred + R_x);

  // GR: Fused x
  xR = x_pred + K_x * innov_x;
  P_x = (1.0f - K_x) * P_x_pred;

  // GR: y-axis kalman filter
  float y_pred = yR + vyIMU * dt;
  float P_y_pred = P_y + Q_y;

  float innov_y = yOdo - y_pred;
  float K_y = P_y_pred / (P_y_pred + R_y);

  // GR: Fused y
  yR = y_pred + K_y * innov_y;
  P_y = (1.0f - K_y) * P_y_pred;