/* Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* * servo.h * * Created on: 2009-apr-25 * Author: Benjamin * * * Changelog: * 2013-12-XX * - Port to ChibiOS and STM32F4 * - probably more changes * * 2011-12-08 * - Renamed functions * - Fixed pulse issue when pulses are close together * * 2011-07-17 * - Changed some variables to volatile. * - Added another command. * * 2010-11-13 * - Calculate the servo timings independent of F_CPU. * - Cleanup * */ #ifndef SERVO_H_ #define SERVO_H_ #include "ch.h" #include "hal.h" #include "hw.h" #include "stm32f4xx_conf.h" #ifndef _BV #define _BV(bit) (1 << (bit)) #endif #ifndef MIN #define MIN(a,b) (((a)<(b))?(a):(b)) #endif #ifndef MAX #define MAX(a,b) (((a)>(b))?(a):(b)) #endif // Change these parameters #define SERVOS_NUM HW_SERVO_NUM // Number of servos to use // Servo timer speed in HZ #define SERVO_CNT_SPEED 1000000L /* * Servo timing (in uS) * * The default signal to drive servos looks like this: * * ----1000us----|----1000us----|---20000us---- * _____________________________ * |______________|_____...______ * * -S_STARTPULSE-|--S_PULSELEN--|--S_COOLDOWN-- * * And the default parameters are the following * #define S_STARTPULSE 1000L * #define S_PULSELEN 1000L * #define S_COOLDOWN 20000L * * You can experiment with these to make your servo move further. * For mg995 these can be WAY out of spec. * * Note that S_PULSELEN is not accurate at all for low F_CPU. However, * it will be rounded up to the nearest possible value (hence the strange * calculation below) * */ #define S_STARTPULSE 1000L #define S_PULSELEN 1000L #define S_COOLDOWN 20000L /* * Dynamic servo parameters * Calculated from F_CPU */ #define SERVO_START_OFFSET (SERVO_CNT_SPEED / (1000000L / S_STARTPULSE)) #define SERVO_CPU_FACTOR ((SERVO_CNT_SPEED + ((1000000L / S_PULSELEN) * 256L) - 1L) / ((1000000L / S_PULSELEN) * 256L)) // Round up #define SERVO_COOLDOWN_FACTOR (SERVO_CNT_SPEED / (1000000L / S_COOLDOWN)) /* * Compile with commands to mode servos with a specified speed * to s specified position interrupt driven. Enabling this will * use some extra ram and a few bytes of flash. */ #define USE_COMMANDS 0 /* * Calculate how many clock cycles it takes to generate PWM. */ #define TEST_CYCLE_TIME 0 #if TEST_CYCLE_TIME extern volatile unsigned int restart_cnt; extern volatile unsigned int interrupt_cnt; #define get_restart_cycles() (restart_cnt) #define get_interrupt_cycles() (interrupt_cnt) #endif // Servo macros #define ACTUAL_POS(servo) (MAX(MIN(servo.pos + (signed short)servo.offset, 255), 0)) #define ACTUAL_POS_PTR(servo) (MAX(MIN(servo->pos + (signed short)servo->offset, 255), 0)) #define CMD_MS_TO_VAL(ms) ((ms) / (((S_STARTPULSE + S_PULSELEN + S_COOLDOWN) * CMD_WAIT_FACTOR) / 1000)) #define SERVO_PERIOD_TIME_MS ((S_STARTPULSE + S_PULSELEN + S_COOLDOWN) / 1000) // Some servo speed defines // TODO typedef struct { volatile signed short pos; volatile unsigned char offset; GPIO_TypeDef* gpio; volatile unsigned int pin; } SERVO; #if USE_COMMANDS typedef struct { volatile signed char active; volatile signed short pos; volatile signed short speed; volatile signed short last; } SERVO_CMD; extern volatile signed char cmd_seq_running; extern volatile unsigned int cmd_ptr; extern volatile const signed short *cmd_seq; /* * The number of servo cycles to wait for each time unit in the wait command. * * The wait time can be calculated with: * (S_STARTPULSE + S_PULSELEN + S_COOLDOWN) * CMD_WAIT_FACTOR * */ #define CMD_WAIT_FACTOR 1 /* * Servo commands. */ /* * Move servo to given position with given speed. * * Param 1: Servo. * Param 2: Position. * Param 3: Speed. 0 for max speed. */ #define CMD_MOVE_SERVO 0 /* * Move servo to given relative position with given speed. * * Param 1: Servo. * Param 2: Relative position. * Param 3: Speed. 0 for max speed. */ #define CMD_MOVE_SERVO_REL 1 /* * Move multiple servos such that they arrive at the same time * * Param 1: Number of servos. * Param 2: Time for movement in milliseconds * Param 3: Servo 1 index * Param 4: Servo 1 pos * Param 5: Servo 2 index * Param 6: Servo 2 pos * ... and so on */ #define CMD_MOVE_MULTIPLE_SERVOS 2 /* * Move all servos to center position * * Param 1: Time for movement in milliseconds */ #define CMD_CENTER_ALL 3 /* * Wait for a while. See configuration for more info. * * Param 1: Time to wait */ #define CMD_WAIT 4 /* * Wait for servo to be ready. * * Param 1: Servo to wait for. */ #define CMD_WAIT_SERVO 5 /* * Wait for all servo commands to get ready. */ #define CMD_WAIT_ALL_SERVOS 6 /* * Stop servo driver. */ #define CMD_STOP_DRIVER 7 /* * End of command. */ #define CMD_STOP_CMDS 8 /* * Restart this command sequence */ #define CMD_RESTART 9 /* * Repeat command sequence a number of times * * Param 1: Number of times to repeat commands. */ #define CMD_REPEAT 10 #endif extern volatile SERVO servos[SERVOS_NUM]; void servo_init(void); void servo_stop_driver(void); unsigned char servo_driver_is_active(void); void servo_irq(void); #if USE_COMMANDS void servo_move(unsigned char servo, signed short position, unsigned char speed); void servo_run_cmds(const signed short *cmds); void servo_stop_cmds(void); void servo_reset_pos(unsigned char speed); void servo_wait_for_cmds(void); void servo_move_within_time(unsigned char servo, signed short pos, unsigned short time_ms); void servo_move_within_time_multiple(unsigned short time_ms, unsigned short num, ...); #endif #endif /* SERVO_H_ */