Skip to content

Commit 646270e

Browse files
laiibailaiibai
committed
feat(your_otto_robot): improve servo control and T5AI_OTTO board integration
- Extend otto_ninja servo app logic and headers - Adjust main flow and app/board default configs - Update T5AI_OTTO Kconfig and board init (t5ai_otto.c) Made-with: Cursor
1 parent 4a60bcf commit 646270e

7 files changed

Lines changed: 298 additions & 51 deletions

File tree

‎apps/tuya.ai/your_otto_robot/app_default.config‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
CONFIG_PROJECT_VERSION="1.0.2"
2-
CONFIG_APP_UI_EYES_240_240=y
2+
CONFIG_TUYA_PRODUCT_ID="b05nl7xoijx4fwae"
3+
CONFIG_APP_UI_EYES_160_160=y
34
CONFIG_BOARD_CHOICE_T5AI=y
45
CONFIG_BOARD_CHOICE_T5AI_OTTO=y
5-
CONFIG_T5AI_OTTO_EX_MODULE_ST7789=y
6+
CONFIG_T5AI_OTTO_EX_MODULE_GC9D01=y
7+
CONFIG_T5AI_OTTO_GC9D01_BL_ACTIVE_LOW=y
68
CONFIG_ENABLE_LIBLVGL=y

‎apps/tuya.ai/your_otto_robot/config/T5AI_OTTO_LCD_GC9D01.config‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,5 @@ CONFIG_APP_UI_EYES_160_160=y
44
CONFIG_BOARD_CHOICE_T5AI=y
55
CONFIG_BOARD_CHOICE_T5AI_OTTO=y
66
CONFIG_T5AI_OTTO_EX_MODULE_GC9D01=y
7-
CONFIG_ENABLE_LIBLVGL=y
7+
CONFIG_T5AI_OTTO_GC9D01_BL_ACTIVE_LOW=y
8+
CONFIG_ENABLE_LIBLVGL=y

‎apps/tuya.ai/your_otto_robot/src/otto_ninja/otto_ninja_app_servo.c‎

Lines changed: 216 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -169,10 +169,10 @@ void platform_tuya_init(void)
169169
#define SERVO_MAX_PULSE 2500
170170

171171
// ==================== Calibration Parameters ====================
172-
#define LFFWRS 20 // Left foot forward rotation speed
173-
#define RFFWRS 20 // Right foot forward rotation speed
174-
#define LFBWRS 20 // Left foot backward rotation speed
175-
#define RFBWRS 20 // Right foot backward rotation speed
172+
#define LFFWRS 20 // Left foot forward rotation speed (demo_ai_otto calibrated)
173+
#define RFFWRS 12 // Right foot forward rotation speed (demo_ai_otto calibrated)
174+
#define LFBWRS 5 // Left foot backward rotation speed (demo_ai_otto calibrated)
175+
#define RFBWRS 5 // Right foot backward rotation speed (demo_ai_otto calibrated)
176176

177177
#define LA0 60 // Left leg standing position
178178
#define RA0 120 // Right leg standing position
@@ -200,6 +200,14 @@ static servo_t servos[MAX_SERVO_COUNT];
200200
// Time control variable
201201
static uint32_t currentmillis1 = 0;
202202

203+
// Rotate spot state variables
204+
static bool sg_rotate_spot_active = false;
205+
/** When true, robot_rotate_spot_update() runs the continuous foot motion (false during init delays). */
206+
static bool sg_rotate_spot_update_ready = false;
207+
static uint16_t sg_right_foot_angle = 90;
208+
static uint32_t sg_last_foot_update = 0;
209+
static uint32_t sg_angle_accumulator = 0;
210+
203211
// ==================== Utility Functions ====================
204212

205213
/**
@@ -322,6 +330,59 @@ void servo_detach(uint8_t pin)
322330
servos[idx].attached = false;
323331
}
324332

333+
void servo_write_smooth(uint8_t pin, uint16_t target_angle, uint16_t step_delay_ms, uint16_t step_size)
334+
{
335+
int idx = get_servo_index(pin);
336+
337+
if (idx < 0) {
338+
servo_attach(pin, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
339+
idx = get_servo_index(pin);
340+
if (idx < 0) {
341+
return;
342+
}
343+
}
344+
345+
if (!servos[idx].attached) {
346+
servo_attach(pin, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
347+
}
348+
349+
if (target_angle > 180) {
350+
target_angle = 180;
351+
}
352+
353+
uint16_t current_angle = servos[idx].current_angle;
354+
if (current_angle == target_angle) {
355+
return;
356+
}
357+
358+
int16_t direction = (target_angle > current_angle) ? 1 : -1;
359+
int16_t angle_diff = (target_angle > current_angle) ? (target_angle - current_angle) : (current_angle - target_angle);
360+
uint16_t steps = (angle_diff + step_size - 1) / step_size;
361+
if (steps == 0) {
362+
steps = 1;
363+
}
364+
365+
for (uint16_t i = 0; i < steps; i++) {
366+
uint16_t step_angle = current_angle + (direction * step_size * (i + 1));
367+
368+
if (direction > 0 && step_angle > target_angle) {
369+
step_angle = target_angle;
370+
}
371+
if (direction < 0 && step_angle < target_angle) {
372+
step_angle = target_angle;
373+
}
374+
375+
servo_write(pin, step_angle);
376+
if (i < steps - 1) {
377+
delay_ms(step_delay_ms);
378+
}
379+
}
380+
381+
if (servos[idx].current_angle != target_angle) {
382+
servo_write(pin, target_angle);
383+
}
384+
}
385+
325386

326387
// ==================== Initialization Functions ====================
327388

@@ -411,32 +472,23 @@ void servo_detach(uint8_t pin)
411472
{
412473
PR_NOTICE("robot_set_walk");
413474
#if ARM_HEAD_ENABLE == 1
414-
// Arms to middle position
415475
servo_attach(SERVO_LEFT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
416476
servo_attach(SERVO_RIGHT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
417-
servo_write(SERVO_LEFT_ARM_PIN, 90);
418-
servo_write(SERVO_RIGHT_ARM_PIN, 90);
477+
servo_write_smooth(SERVO_LEFT_ARM_PIN, 90, 15, 2);
478+
servo_write_smooth(SERVO_RIGHT_ARM_PIN, 90, 15, 2);
419479
delay_ms(200);
420-
servo_detach(SERVO_LEFT_ARM_PIN);
421-
servo_detach(SERVO_RIGHT_ARM_PIN);
422480
#endif
423-
// Ankles to standing position
424481
servo_attach(SERVO_LEFT_LEG_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
425482
servo_attach(SERVO_RIGHT_LEG_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
426-
servo_write(SERVO_LEFT_LEG_PIN, LA0);
427-
servo_write(SERVO_RIGHT_LEG_PIN, RA0);
483+
servo_write_smooth(SERVO_LEFT_LEG_PIN, LA0, 15, 2);
484+
servo_write_smooth(SERVO_RIGHT_LEG_PIN, RA0, 15, 2);
428485
delay_ms(100);
429-
//servo_detach(SERVO_LEFT_LEG_PIN);
430-
//servo_detach(SERVO_RIGHT_LEG_PIN);
431486

432487
#if ARM_HEAD_ENABLE == 1
433-
// Arms to final position
434488
servo_attach(SERVO_LEFT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
435489
servo_attach(SERVO_RIGHT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
436-
servo_write(SERVO_LEFT_ARM_PIN, 180);
437-
servo_write(SERVO_RIGHT_ARM_PIN, 0);
438-
servo_detach(SERVO_LEFT_ARM_PIN);
439-
servo_detach(SERVO_RIGHT_ARM_PIN);
490+
servo_write_smooth(SERVO_LEFT_ARM_PIN, 180, 15, 2);
491+
servo_write_smooth(SERVO_RIGHT_ARM_PIN, 0, 15, 2);
440492
#endif
441493
}
442494

@@ -447,33 +499,24 @@ void servo_detach(uint8_t pin)
447499
{
448500
PR_NOTICE("robot_set_roll");
449501
#if ARM_HEAD_ENABLE == 1
450-
// Arms to middle position
451502
servo_attach(SERVO_LEFT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
452503
servo_attach(SERVO_RIGHT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
453-
servo_write(SERVO_LEFT_ARM_PIN, 90);
454-
servo_write(SERVO_RIGHT_ARM_PIN, 90);
504+
servo_write_smooth(SERVO_LEFT_ARM_PIN, 90, 15, 2);
505+
servo_write_smooth(SERVO_RIGHT_ARM_PIN, 90, 15, 2);
455506
delay_ms(200);
456-
servo_detach(SERVO_LEFT_ARM_PIN);
457-
servo_detach(SERVO_RIGHT_ARM_PIN);
458507
#endif
459508

460-
// Ankles to roll position
461509
servo_attach(SERVO_LEFT_LEG_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
462510
servo_attach(SERVO_RIGHT_LEG_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
463-
servo_write(SERVO_LEFT_LEG_PIN, LA1);
464-
servo_write(SERVO_RIGHT_LEG_PIN, RA1);
511+
servo_write_smooth(SERVO_LEFT_LEG_PIN, LA1, 20, 3);
512+
servo_write_smooth(SERVO_RIGHT_LEG_PIN, RA1, 20, 3);
465513
delay_ms(100);
466-
// servo_detach(SERVO_LEFT_LEG_PIN);
467-
// servo_detach(SERVO_RIGHT_LEG_PIN);
468514

469515
#if ARM_HEAD_ENABLE == 1
470-
// Arms to final position
471516
servo_attach(SERVO_LEFT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
472517
servo_attach(SERVO_RIGHT_ARM_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
473-
servo_write(SERVO_LEFT_ARM_PIN, 180);
474-
servo_write(SERVO_RIGHT_ARM_PIN, 0);
475-
servo_detach(SERVO_LEFT_ARM_PIN);
476-
servo_detach(SERVO_RIGHT_ARM_PIN);
518+
servo_write_smooth(SERVO_LEFT_ARM_PIN, 180, 15, 2);
519+
servo_write_smooth(SERVO_RIGHT_ARM_PIN, 0, 15, 2);
477520
#endif
478521
}
479522

@@ -508,9 +551,9 @@ void servo_detach(uint8_t pin)
508551
{
509552
if (joystick_y <= 0) return; // Only process forward
510553

511-
// Calculate left/right turn time
512-
int lt = map_value(joystick_x, 100, -100, 200, 700);
513-
int rt = map_value(joystick_x, 100, -100, 700, 200);
554+
// Calibrated timing from demo_ai_otto.
555+
int lt = map_value(joystick_x, 100, -100, 300, 500);
556+
int rt = map_value(joystick_x, 100, -100, 400, 600);
514557
// PR_NOTICE("robot_walk_forward: lt=%d, rt=%d", lt, rt);
515558
// Calculate time intervals
516559
int interval1 = 250;
@@ -582,9 +625,9 @@ void servo_detach(uint8_t pin)
582625
{
583626
if (joystick_y >= 0) return; // Only process backward
584627

585-
// Calculate left/right turn time
586-
int lt = map_value(joystick_x, 100, -100, 200, 700);
587-
int rt = map_value(joystick_x, 100, -100, 700, 200);
628+
// Calibrated timing from demo_ai_otto.
629+
int lt = map_value(joystick_x, 100, -100, 150, 650);
630+
int rt = map_value(joystick_x, 100, -100, 750, 250);
588631

589632
// Calculate time intervals
590633
int interval1 = 250;
@@ -680,6 +723,131 @@ void servo_detach(uint8_t pin)
680723
servo_write(SERVO_RIGHT_FOOT_PIN, right_wheel_speed + right_wheel_delta);
681724
//PR_NOTICE("robot_roll_control: right_wheel_speed+right_wheel_delta=%d", right_wheel_speed+right_wheel_delta);
682725
}
726+
727+
void robot_rotate_spot(bool direction)
728+
{
729+
PR_NOTICE("robot_rotate_spot: direction=%s, starting rotation", direction ? "right" : "left");
730+
731+
/*
732+
* Set active before init delays so main_loop does not call robot_walk_stop() / roll control
733+
* in parallel (DP may run on another thread). update_ready stays false until init completes.
734+
*/
735+
sg_rotate_spot_active = true;
736+
sg_rotate_spot_update_ready = false;
737+
738+
if (get_mode_counter() != 0) {
739+
PR_NOTICE("robot_rotate_spot: Not in walk mode, switching to walk mode");
740+
robot_set_walk();
741+
delay_ms(500);
742+
}
743+
744+
servo_attach(SERVO_LEFT_LEG_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
745+
servo_attach(SERVO_RIGHT_LEG_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
746+
servo_attach(SERVO_RIGHT_FOOT_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
747+
servo_attach(SERVO_LEFT_FOOT_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
748+
749+
servo_write(SERVO_LEFT_LEG_PIN, LATR);
750+
servo_write(SERVO_RIGHT_LEG_PIN, RATR);
751+
delay_ms(250);
752+
753+
uint16_t start_angle = 90 - RFFWRS;
754+
servo_write(SERVO_RIGHT_FOOT_PIN, start_angle);
755+
delay_ms(500);
756+
757+
sg_right_foot_angle = start_angle;
758+
sg_last_foot_update = get_millis();
759+
760+
if (sg_right_foot_angle < 60) {
761+
sg_right_foot_angle = 60;
762+
}
763+
if (sg_right_foot_angle > 180) {
764+
sg_right_foot_angle = 180;
765+
}
766+
if (sg_right_foot_angle <= 120) {
767+
sg_angle_accumulator = (sg_right_foot_angle - 60) * 9000 / 60;
768+
} else {
769+
sg_angle_accumulator = 9000 + (sg_right_foot_angle - 120) * 9000 / 60;
770+
}
771+
772+
sg_rotate_spot_update_ready = true;
773+
}
774+
775+
void robot_rotate_spot_stop(void)
776+
{
777+
PR_NOTICE("robot_rotate_spot_stop: Stopping rotation");
778+
sg_rotate_spot_active = false;
779+
sg_rotate_spot_update_ready = false;
780+
781+
servo_write(SERVO_LEFT_LEG_PIN, LA0);
782+
servo_write(SERVO_RIGHT_LEG_PIN, RA0);
783+
servo_write(SERVO_RIGHT_FOOT_PIN, 90);
784+
785+
servo_detach(SERVO_LEFT_LEG_PIN);
786+
servo_detach(SERVO_RIGHT_LEG_PIN);
787+
servo_detach(SERVO_RIGHT_FOOT_PIN);
788+
servo_detach(SERVO_LEFT_FOOT_PIN);
789+
}
790+
791+
void robot_rotate_spot_update(void)
792+
{
793+
if (!sg_rotate_spot_active) {
794+
return;
795+
}
796+
if (!sg_rotate_spot_update_ready) {
797+
return;
798+
}
799+
800+
uint32_t current_time = get_millis();
801+
uint32_t elapsed = current_time - sg_last_foot_update;
802+
803+
if (elapsed >= 3) {
804+
sg_last_foot_update = current_time;
805+
806+
uint32_t increment = (elapsed * 15) / 3;
807+
if (increment < 1) {
808+
increment = 1;
809+
}
810+
if (increment > 50) {
811+
increment = 50;
812+
}
813+
814+
sg_angle_accumulator += increment;
815+
if (sg_angle_accumulator >= 36000) {
816+
sg_angle_accumulator %= 36000;
817+
}
818+
819+
uint32_t phase = sg_angle_accumulator % 36000;
820+
uint32_t quarter = phase / 9000;
821+
uint32_t pos_in_quarter = phase % 9000;
822+
823+
switch (quarter) {
824+
case 0:
825+
sg_right_foot_angle = 60 + (pos_in_quarter * 60) / 9000;
826+
break;
827+
case 1:
828+
sg_right_foot_angle = 120 + (pos_in_quarter * 60) / 9000;
829+
break;
830+
case 2:
831+
sg_right_foot_angle = 180 - (pos_in_quarter * 60) / 9000;
832+
break;
833+
case 3:
834+
sg_right_foot_angle = 120 - (pos_in_quarter * 60) / 9000;
835+
break;
836+
default:
837+
sg_right_foot_angle = 90;
838+
break;
839+
}
840+
841+
if (sg_right_foot_angle < 60) {
842+
sg_right_foot_angle = 60;
843+
}
844+
if (sg_right_foot_angle > 180) {
845+
sg_right_foot_angle = 180;
846+
}
847+
848+
servo_write(SERVO_RIGHT_FOOT_PIN, sg_right_foot_angle);
849+
}
850+
}
683851

684852
/**
685853
* Left arm up
@@ -745,6 +913,14 @@ void main_loop(void)
745913
PR_NOTICE("robot_set_roll");
746914
}
747915
}
916+
917+
robot_rotate_spot_update();
918+
919+
/* While spot rotation is armed or running, do not run walk/roll (may run on another thread). */
920+
if (sg_rotate_spot_active) {
921+
return;
922+
}
923+
748924
// Execute different motion control based on mode
749925
if (get_mode_counter() == 0) {
750926
// Walk mode
@@ -792,4 +968,3 @@ void main_init(void)
792968
// Other initialization code...
793969
}
794970

795-

‎apps/tuya.ai/your_otto_robot/src/otto_ninja/otto_ninja_app_servo.h‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,12 @@ void platform_tuya_init(void);
4444
*/
4545
void servo_control_init(void);
4646

47+
/**
48+
* Smooth servo angle transition
49+
* Gradually transition from current angle to target angle to avoid sudden changes
50+
*/
51+
void servo_write_smooth(uint8_t pin, uint16_t target_angle, uint16_t step_delay_ms, uint16_t step_size);
52+
4753
void main_init(void);
4854
void main_loop(void);
4955

@@ -58,5 +64,7 @@ void robot_right_arm_down(void);
5864
void robot_roll_control(int8_t joystick_x, int8_t joystick_y);
5965
void robot_walk_forward(int8_t joystick_x, int8_t joystick_y);
6066
void robot_walk_backward(int8_t joystick_x, int8_t joystick_y);
67+
void robot_rotate_spot(bool direction);
68+
void robot_rotate_spot_stop(void);
69+
void robot_rotate_spot_update(void);
6170
#endif // OTTO_NINJA_APP_SERVO_H
62-

0 commit comments

Comments
 (0)