servo.h

/*
	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 <http://www.gnu.org/licenses/>.
    */

/*
 * 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 1

/*
 * 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_ */