diff --git a/robot/include/servos.h b/robot/include/servos.h index 4ede364..ea0c094 100644 --- a/robot/include/servos.h +++ b/robot/include/servos.h @@ -26,6 +26,13 @@ #define US2TIMER0(us) (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS)) void init_servos(void); -void run_servos(void); + +void servo_full_forw(void); +void servo_turn_forw_r(void); +void servo_turn_forw_l(void); +void servo_full_backw(void); +void servo_turn_backw_r(void); +void servo_turn_backw_l(void); + #endif /* SERVOS_H */ diff --git a/robot/src/servos.c b/robot/src/servos.c index 0f6bd11..778b0b1 100644 --- a/robot/src/servos.c +++ b/robot/src/servos.c @@ -23,8 +23,38 @@ void init_servos(void) SET(SERVO_DDR, SERVO_R_BIT); } -void run_servos(void) +void servo_full_forw(void) { SERVO_L_REG = US2TIMER0(SERVO_L_FORW); SERVO_R_REG = US2TIMER0(SERVO_R_FORW); } + +void servo_turn_forw_r(void) +{ + SERVO_L_REG = US2TIMER0(SERVO_L_FORW); + SERVO_R_REG = US2TIMER0(SERVO_R_STOP); +} + +void servo_turn_forw_l(void) +{ + SERVO_L_REG = US2TIMER0(SERVO_L_STOP); + SERVO_R_REG = US2TIMER0(SERVO_R_FORW); +} + +void servo_full_backw(void) +{ + SERVO_L_REG = US2TIMER0(SERVO_L_BACKW); + SERVO_R_REG = US2TIMER0(SERVO_R_BACKW); +} + +void servo_turn_backw_r(void) +{ + SERVO_L_REG = US2TIMER0(SERVO_L_STOP); + SERVO_R_REG = US2TIMER0(SERVO_R_BACKW); +} + +void servo_turn_backw_l(void) +{ + SERVO_L_REG = US2TIMER0(SERVO_L_BACKW); + SERVO_R_REG = US2TIMER0(SERVO_R_STOP); +}