WebSVN - Projects - Blame - Rev 1687 - /NGVideo5

Rev	Author	Line No.	Line
1687	-	1
		2	/********************************************************************/
		3	/* */
		4	/* NG-Video 5,8GHz */
		5	/* */
		6	/* Copyright (C) 2011 - gebad */
		7	/* */
		8	/* This code is distributed under the GNU Public License */
		9	/* which can be found at http://www.gnu.org/licenses/gpl.txt */
		10	/* */
		11	/* using */
		12	/! \file servo.c \brief Interrupt-driven RC Servo function library./
		13	/* */
		14	/File Name : 'servo.c' /
		15	/Title : Interrupt-driven RC Servo function library /
		16	/Author : Pascal Stang - Copyright (C) 2002 /
		17	/Created : 7/31/2002 /
		18	/Revised : 8/02/2002 /
		19	/Version : 1.0 /
		20	/Target MCU : Atmel AVR Series /
		21	/Editor Tabs : 2 /
		22	/* */
		23	/This code is distributed under the GNU Public License /
		24	/* which can be found at http://www.gnu.org/licenses/gpl.txt */
		25	/* */
		26	/********************************************************************/
		27
		28	#include <avr/interrupt.h>
		29	#include <avr/io.h>
		30
		31	#include "servo.h"
		32	#include "config.h"
		33
		34	// Global variables
		35	uint16_t ServoPosTics;
		36	uint16_t ServoPeriodTics;
		37
		38	// servo channel registers
		39	ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
		40
		41	const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER},
		42	{SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}};
		43
		44	// Servo limits (depend on hardware)
		45	const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX},
		46	{SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX},
		47	{SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}},
		48	{{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX},
		49	{SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX},
		50	{SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}};
		51
		52	// Servopositionen normiert f�r 950�s, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in �s ist F_CPU*e-1
		53	const steps_pw_t steps_pw[2] = {{(uint64_t)950F_CPU1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050F_CPU1e-6/SERVO_PRESCALER + 0.5, (uint64_t)1500F_CPU1e-6/SERVO_PRESCALER + 0.5},
		54	{(uint64_t)9504F_CPU1e-6/SERVO_PRESCALER + 0.5, (uint64_t)20504F_CPU1e-6/SERVO_PRESCALER + 0.5, (uint64_t)15004F_CPU*1e-6/SERVO_PRESCALER + 0.5}};
		55
		56	// anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX
		57	const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 };
		58
		59	// functions
		60
		61	//! initializes software PWM system 16-bit Timer
		62	// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt
		63	// Werte: rev, min, max, mid vorher �ber servoSet...() initialisieren und einmal servoSetPosition(...) ausf�hren!!!
		64	void servoInit(uint8_t servo_period)
		65	{ uint16_t OCValue; // set initial interrupt time
		66
		67	// disble Timer/Counter1, Output Compare A Match Interrupt
		68	TIMSK1 &= ~(1<<OCIE1A);
		69	// set the prescaler for timer1
		70	if (sIdxSteps == STEPS_255) {
		71	TCCR1B &= ~((1<<CS11) \| (1<<CS10));
		72	TCCR1B \|= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel
		73	}
		74	else {
		75	TCCR1B &= ~(1<<CS12);
		76	TCCR1B \|= (1<<CS11) \| (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel
		77	}
		78	// attach the software PWM service routine to timer1 output compare A
		79	// timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
		80	// enable channels
		81	for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) {
		82	// set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5
		83	ServoChannels[channel].pin = (1 << (SERVO2 + channel));
		84	}
		85	ServoPosTics = 0; // set PosTics
		86	// set PeriodTics
		87	ServoPeriodTics = F_CPU / ServoConst[sIdxSteps].prescaler * servo_period * 1e-3;
		88	// read in current value of output compare register OCR1A
		89	OCValue = OCR1AL; // read low byte of OCR1A
		90	OCValue += (OCR1AH << 8); // read high byte of OCR1A
		91	OCValue += ServoPeriodTics; // increment OCR1A value by nextTics
		92	// set future output compare time to this new value
		93	OCR1AH = OCValue >> 8; // write high byte
		94	OCR1AL = OCValue & 0x00FF; // write low byte
		95	TIMSK1 \|= (1<<OCIE1A); // enable the timer1 output compare A interrupt
		96	}
		97
		98	uint16_t pw_us(uint16_t Steps)
		99	{
		100	return(Steps * ServoConst[sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in �s
		101	}
		102
		103	uint16_t ServoSteps(void)
		104	{
		105	return(ServoConst[sIdxSteps].steps);
		106	}
		107
		108	void servoSet_rev(uint8_t channel, uint8_t val)
		109	{
		110	ServoChannels[channel].rev = val & 0x01;
		111	}
		112
		113	void servoSet_min(uint8_t channel, uint16_t min)
		114	{
		115	ServoChannels[channel].min = ServoConst[sIdxSteps].min + min;
		116	}
		117
		118	void servoSet_max(uint8_t channel, uint16_t max)
		119	{
		120	ServoChannels[channel].max = ServoConst[sIdxSteps].max - max;
		121	}
		122
		123	void servoSet_mid(uint8_t channel, uint16_t mid)
		124	{
		125	ServoChannels[channel].mid = mid;
		126	// Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen
		127	ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \
		128	(16 * mid - 8 * ServoConst[sIdxSteps].steps))/16 + ServoChannels[channel].min;
		129	}
		130
		131	//! turns off software PWM system
		132	void servoOff(void)
		133	{
		134	// disable the timer1 output compare A interrupt
		135	TIMSK1 &= ~(1<<OCIE1A);
		136	}
		137
		138	//! set servo position on channel (raw unscaled format)
		139	void servoSetPositionRaw(uint8_t channel, uint16_t position)
		140	{
		141	// bind to limits
		142	if (position < ServoChannels[channel].min) position = ServoChannels[channel].min;
		143	if (position > ServoChannels[channel].max) position = ServoChannels[channel].max;
		144	// set position
		145	ServoChannels[channel].duty = position;
		146	}
		147
		148	//! set servo position on channel
		149	// input should be between 0 and ServoSteps() (entspricht Maximalausschlag)
		150	void servoSetPosition(uint8_t channel, uint16_t position)
		151	{ uint16_t pos_scaled;
		152
		153	// calculate scaled position
		154	if (ServoChannels[channel].rev != 0) position = ServoConst[sIdxSteps].steps - position;
		155	if (position < ServoConst[sIdxSteps].steps/2)
		156	//bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2)
		157	pos_scaled = ServoChannels[channel].min + ((int32_t)position2(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \
		158	ServoConst[sIdxSteps].steps;
		159	else
		160	//bei Position >= Servomittelstellung
		161	pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[sIdxSteps].steps / 2) \
		162	* 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[sIdxSteps].steps;
		163	// set position
		164	servoSetPositionRaw(channel, pos_scaled);
		165	}
		166
		167	// Umrechnung Winkel in Servoschritte
		168	void servoSetAngle(uint8_t servo_nr, int16_t angle)
		169	{
		170	servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[sIdxSteps].steps / 180));
		171	}
		172
		173	//Interruptroutine
		174	ISR(TIMER1_COMPA_vect)
		175	{ static uint8_t ServoChannel;
		176	uint16_t nextTics;
		177	uint16_t OCValue; // schedule next interrupt
		178
		179	if(ServoChannel < SERVO_NUM_CHANNELS) {
		180	PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel
		181	}
		182	ServoChannel++; // next channel
		183	if(ServoChannel != SERVO_NUM_CHANNELS) {
		184	// loop to channel 0 if needed
		185	if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0;
		186	// turn on new channel
		187	PORTD \|= ServoChannels[ServoChannel].pin;
		188	// schedule turn off time
		189	nextTics = ServoChannels[ServoChannel].duty;
		190	}
		191	else {
		192	// ***we could save time by precalculating this
		193	// schedule end-of-period
		194	nextTics = ServoPeriodTics-ServoPosTics;
		195	}
		196	// read in current value of output compare register OCR1A
		197	OCValue = OCR1AL; // read low byte of OCR1A
		198	OCValue += (OCR1AH <<8); // read high byte of OCR1A
		199	OCValue += nextTics; // increment OCR1A value by nextTics
		200	// set future output compare time to this new value
		201	OCR1AH = OCValue >> 8; // write high byte
		202	OCR1AL = OCValue & 0x00FF; // write low byte
		203
		204	ServoPosTics += nextTics; // set our new tic position
		205	if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
		206	}
		207

Subversion Repositories Projects

(root)/NGVideo5_8/tags/v1.33/servo.c - Rev 1687