
#include <Arduino.h>
#include <oseppRobot.h>

OseppRangeFinder U(2);
OseppTBMotor L(12, 11, LOW);
OseppTBMotor R(8, 3);
OseppRemote remoteController;

boolean enableGravity = false;

void setup() {
    Serial.begin(9600);
    Serial.println("OSEPP机器人准备就绪!");
    Serial.println("[L][U][R][D] 和左边摇杆");
    Serial.println("控制小车移动");
    Serial.println("右边摇杆控制机械爪");
    Serial.println("[X] 键启用重力感应控制");
    Serial.println("[B] 键关闭重力感应控制");
}

int x, y, z;
int l, r;

void loop() {
    if (U.ping() < 350) {
        Serial.print("障碍物距离");
        Serial.print(U.ping());
        Serial.println("毫米");
    }
    if (remoteController.getButtonState(button_X)) {
        if (!enableGravity) {
            enableGravity = true;
            Serial.println("重力感应控制已开启");
        }
    } else if (remoteController.getButtonState(button_B)) {
        if (enableGravity) {
            enableGravity = false;
            Serial.println("重力感应控制已关闭");
        }
    }
    if (remoteController.isTimeoutFor(500)) {
        L.forward(0);
        R.forward(0);
        Serial.println("丢失遥控器信号");
        delay(500);
    } else if (remoteController.getButtonState(button_D)) {
        L.backward(200);
        R.backward(200);
    } else if (remoteController.getButtonState(button_L)) {
        L.backward(200);
        R.forward(200);
    } else if (remoteController.getButtonState(button_R)) {
        L.forward(200);
        R.backward(200);
    } else if (remoteController.getButtonState(button_U)) {
        L.forward(200);
        R.forward(200);
    } else {
        x = remoteController.getChannalData(left_X);
        y = remoteController.getChannalData(left_Y);
        if (x == 0 && y == 0 && enableGravity) {
            x = -remoteController.getChannalData(gravity_Y);
            y = remoteController.getChannalData(gravity_X);
            z = remoteController.getChannalData(gravity_Z);
        }
        l = r = y;
        x /= 2;
        l += x;
        r -= x;
        int m = max(abs(l), abs(r));
        if (m > 255) {
            l = map(l, -m, m, -255, 255);
            r = map(r, -m, m, -255, 255);
        }
        L.forward(l);
        R.forward(r);
    }
    delay(25);
}
