diff --git a/src/Stackchan_servo.cpp b/src/Stackchan_servo.cpp index c4fd42a..53fd01b 100644 --- a/src/Stackchan_servo.cpp +++ b/src/Stackchan_servo.cpp @@ -72,7 +72,7 @@ void StackchanSERVO::attachServos() { _dxl.writeControlTableItem(DRIVE_MODE, AXIS_X + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更 _dxl.writeControlTableItem(DRIVE_MODE, AXIS_Y + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更 _dxl.torqueOn(AXIS_X + 1); - delay(10); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。 + delay(100); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。 _dxl.torqueOn(AXIS_Y + 1); delay(100); _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 1000); @@ -103,13 +103,18 @@ void StackchanSERVO::attachServos() { _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, 1000); delay(100); + M5_LOGI("CurrentPosition X:%f, Y:%f", _dxl.getPresentPosition(AXIS_X + 1), _dxl.getPresentPosition(AXIS_Y + 1)); + if (_dxl.getPresentPosition(AXIS_X + 1) > 4096) { _init_param.servo[AXIS_X].offset = _init_param.servo[AXIS_X].offset + 360; } - if (_dxl.getPresentPosition(AXIS_Y + 1) > 4096) { + if ((_dxl.getPresentPosition(AXIS_Y + 1)-convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].lower_limit + _init_param.servo[AXIS_Y].offset)) > convertDYNIXELXL330_RT(270)) { _init_param.servo[AXIS_Y].offset = _init_param.servo[AXIS_Y].offset + 360; } + //_init_param.servo[AXIS_Y].offset = 360; + M5_LOGI("Current Offset X:%d, Y:%d", _init_param.servo[AXIS_X].offset, _init_param.servo[AXIS_Y].offset); + _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset)); _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset)); //_dxl.torqueOff(AXIS_X + 1); diff --git a/src/Stackchan_servo.h b/src/Stackchan_servo.h index d36c2aa..bba8810 100644 --- a/src/Stackchan_servo.h +++ b/src/Stackchan_servo.h @@ -44,8 +44,8 @@ typedef struct ServoParam { int16_t offset; // オフセット(90°からの+-) int16_t degree; // 角度 uint32_t millis_for_move; // 移動時間(msec) - uint16_t lower_limit; // サーボ角度の下限 - uint16_t upper_limit; // サーボ角度の上限 + int16_t lower_limit; // サーボ角度の下限 + int16_t upper_limit; // サーボ角度の上限 } servo_param_s; diff --git a/src/Stackchan_system_config.h b/src/Stackchan_system_config.h index d3e52f0..baa5142 100644 --- a/src/Stackchan_system_config.h +++ b/src/Stackchan_system_config.h @@ -41,10 +41,10 @@ typedef struct SecretItems { typedef struct ServoInitialParam { uint8_t pin; - uint16_t offset; - uint16_t upper_limit; - uint16_t lower_limit; - uint16_t start_degree; + int16_t offset; + int16_t upper_limit; + int16_t lower_limit; + int16_t start_degree; } servo_initial_param_s; enum AvatarMode {