diff --git a/src/accel.h b/src/accel.h index 646feda..712409d 100644 --- a/src/accel.h +++ b/src/accel.h @@ -12,8 +12,8 @@ int16_t cels = 0; void initAccel() { - return; - Wire.begin(); + // return; + // Wire.begin(); Wire.beginTransmission(0x68); // I2C address of the MPU-6050 Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) @@ -22,7 +22,7 @@ void initAccel() void readAccel() { - return; + // return; Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); diff --git a/src/remote.cpp b/src/remote.cpp index d1f6f7f..1bd438b 100644 --- a/src/remote.cpp +++ b/src/remote.cpp @@ -49,7 +49,8 @@ uint8_t boardData[6] = {0, 0, 0, 0, 0, 0}; // #define steeringInfluential -uint8_t currentMode = M_NORMAL; +#define DEFAULT_MODE M_STEERING +uint8_t currentMode = DEFAULT_MODE; // Default to steering in this case uint8_t selectedIndex = 2; bool lightActive = false; @@ -118,8 +119,6 @@ bool triggerActive(); #include "settings.h" -uint8_t *buf; - void setCrusing(int16_t speed) { if (speed < THROTTLE_CENTER) @@ -570,14 +569,14 @@ void checkClicks(void *parameter) { DEBUG_PRINTLN("YEAH TRIPPLE"); - if (currentMode == M_NORMAL) + if (currentMode == DEFAULT_MODE) { currentMode = M_SELECT; selectedIndex = 2; } else { - currentMode = M_NORMAL; + currentMode = DEFAULT_MODE; } } clickCounter = 0; @@ -602,15 +601,15 @@ void checkClicks(void *parameter) else settings[limitMode] = 0; shouldUpdateSettings = true; - currentMode = M_NORMAL; + currentMode = DEFAULT_MODE; break; case 2: lightActive = !lightActive; updateBoardOptions(); - currentMode = M_NORMAL; + currentMode = DEFAULT_MODE; break; case 3: - currentMode = M_STEERING; + currentMode = M_NORMAL; // onvert this break; case 4: currentMode = M_CRUISE; @@ -1047,18 +1046,10 @@ void setup() &clickTaskHandle, 0); - initAccel(); - DEBUG_PRINTLN("ESPNowSkate Sender"); - u8g2.setBusClock(400000); u8g2.begin(); - u8g2.setBusClock(400000); - // buf = (uint8_t *)malloc(u8g2.getBufferSize()); - // u8g2.setBufferPtr(buf); - // u8g2.initDisplay(); - // u8g2.clearDisplay(); - // u8g2.setPowerSave(0); + initAccel(); drawStartScreen(); @@ -1140,33 +1131,37 @@ void loop() if (currentMode == M_STEERING) { DEBUG_PRINT("Value: "); - int map = c_map(AcZ, -15000, 15000, -60, 60); - DEBUG_PRINTLN(map); + int steer = c_map(AcZ, -15000, 15000, -500, 500); + steer *= -1; // Invert + DEBUG_PRINTLN(steer); -#ifdef steeringInfluential - esc1 = (uint8_t)constrain(throttle - map, THROTTLE_MIN, THROTTLE_MAX); - esc2 = (uint8_t)constrain(throttle + map, THROTTLE_MIN, THROTTLE_MAX); -#else - esc1 = (uint8_t)constrain(THROTTLE_CENTER - map, THROTTLE_MIN, THROTTLE_MAX); - esc2 = (uint8_t)constrain(THROTTLE_CENTER + map, THROTTLE_MIN, THROTTLE_MAX); - if (-10 < map && map < 10) - { - esc1 = throttle; - esc2 = throttle; - } + // #ifdef steeringInfluential + // esc1 = (uint8_t)constrain(throttle - map, THROTTLE_MIN, THROTTLE_MAX); + // esc2 = (uint8_t)constrain(throttle + map, THROTTLE_MIN, THROTTLE_MAX); + // #else + // esc1 = (uint8_t)constrain(THROTTLE_CENTER - map, THROTTLE_MIN, THROTTLE_MAX); + // esc2 = (uint8_t)constrain(THROTTLE_CENTER + map, THROTTLE_MIN, THROTTLE_MAX); + // if (-10 < map && map < 10) + // { + // esc1 = throttle; + // esc2 = throttle; + // } - // ignore reverse - if (throttle < THROTTLE_CENTER) - { - esc1 = throttle; - esc2 = throttle; - } -#endif - if (throttle == THROTTLE_CENTER) - { - esc1 = THROTTLE_CENTER; - esc2 = THROTTLE_CENTER; - } + // // ignore reverse + // if (throttle < THROTTLE_CENTER) + // { + // esc1 = throttle; + // esc2 = throttle; + // } + // #endif + // if (throttle == THROTTLE_CENTER) + // { + // esc1 = THROTTLE_CENTER; + // esc2 = THROTTLE_CENTER; + // } + + // mixerFcn(speed << 4, steer << 4, &esc1, &esc2); // This function implements the equations above + mixerFcn(throttle << 4, steer << 4, &esc1, &esc2); // This function implements the equations above DEBUG_PRINT("SteeringMode: ESC1: "); DEBUG_PRINT(esc1); diff --git a/src/valuehelpers.h b/src/valuehelpers.h index 85da788..0ba60a7 100644 --- a/src/valuehelpers.h +++ b/src/valuehelpers.h @@ -17,3 +17,32 @@ uint8_t split16(uint16_t value, uint8_t index) } return 0; } +#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) + +#define DEFAULT_SPEED_COEFFICIENT 16384 // Default for SPEED_COEFFICIENT 1.0f [-] higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 +#define DEFAULT_STEER_COEFFICIENT 8192 // Defualt for STEER_COEFFICIENT 0.5f [-] higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 8192 = 0.5 * 2^14. If you do not want any steering, set it to 0. + +/* mixerFcn(rtu_speed, rtu_steer, &rty_speedR, &rty_speedL); + * Inputs: rtu_speed, rtu_steer = fixdt(1,16,4) + * Outputs: rty_speedR, rty_speedL = int16_t + * Parameters: SPEED_COEFFICIENT, STEER_COEFFICIENT = fixdt(0,16,14) + */ +void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t *rty_speedL) +{ + int16_t prodSpeed; + int16_t prodSteer; + int32_t tmp; + + prodSpeed = (int16_t)((rtu_speed * (int16_t)DEFAULT_SPEED_COEFFICIENT) >> 14); + prodSteer = (int16_t)((rtu_steer * (int16_t)DEFAULT_STEER_COEFFICIENT) >> 14); + + tmp = prodSpeed - prodSteer; + tmp = CLAMP(tmp, -32768, 32767); // Overflow protection + *rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int + *rty_speedR = CLAMP(*rty_speedR, -1000, 1000); // TODO: Use constants here + + tmp = prodSpeed + prodSteer; + tmp = CLAMP(tmp, -32768, 32767); // Overflow protection + *rty_speedL = (int16_t)(tmp >> 4); // Convert from fixed-point to int + *rty_speedL = CLAMP(*rty_speedL, -1000, 1000); // TODO: Use constants here +}