WebSVN - Projects - Blame - /Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/ng

Rev	Author	Line No.	Line
1915	-	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 "../tracking/ng_servo.h"
		32	#include "../tracking/tracking.h"
		33	#include "../HAL_HW3_9.h"
		34	#include "../eeprom/eeprom.h"
		35	//#include "config.h"
		36
		37	// Global variables
		38	uint16_t ServoPosTics;
		39	uint16_t ServoPeriodTics;
		40
		41	// servo channel registers
		42	ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
		43
		44	const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER},
		45	{SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}};
		46
		47	// Servo limits (depend on hardware)
		48	const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX},
		49	{SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX},
		50	{SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}},
		51	{{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX},
		52	{SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX},
		53	{SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}};
		54
		55	// Servopositionen normiert f�r 950�s, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in �s ist F_CPU*e-1
		56	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},
		57	{(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}};
		58
		59	// anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX
		60	const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 };
		61
		62	// functions
		63	void servo_test(void)
		64	{
		65	//Dummy
		66	}
		67	//! initializes software PWM system 16-bit Timer
		68	// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt
		69	// Werte: rev, min, max, mid vorher �ber servoSet...() initialisieren und einmal servoSetPosition(...) ausf�hren!!!
		70	void servoInit(uint8_t servo_period)
		71	{ uint16_t OCValue; // set initial interrupt time
		72
		73
		74
		75
		76	// disble Timer/Counter1, Output Compare A Match Interrupt
		77	TIMSK1 &= ~(1<<OCIE1A);
		78	// set the prescaler for timer1
		79	if (Config.sIdxSteps == STEPS_255) {
		80	TCCR1B &= ~((1<<CS11) \| (1<<CS10));
		81	TCCR1B \|= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel
		82	}
		83	else {
		84	TCCR1B &= ~(1<<CS12);
		85	TCCR1B \|= (1<<CS11) \| (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel
		86	}
		87	// attach the software PWM service routine to timer1 output compare A
		88	// timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
		89	// enable channels
		90	for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) {
		91	// set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5
		92	ServoChannels[channel].pin = (1 << (SERVO2 + channel));
		93	}
		94	ServoPosTics = 0; // set PosTics
		95	// set PeriodTics
		96	ServoPeriodTics = F_CPU / ServoConst[Config.sIdxSteps].prescaler * servo_period * 1e-3;
		97	// read in current value of output compare register OCR1A
		98	OCValue = OCR1AL; // read low byte of OCR1A
		99	OCValue += (OCR1AH << 8); // read high byte of OCR1A
		100	OCValue += ServoPeriodTics; // increment OCR1A value by nextTics
		101	// set future output compare time to this new value
		102	OCR1AH = OCValue >> 8; // write high byte
		103	OCR1AL = OCValue & 0x00FF; // write low byte
		104	TIMSK1 \|= (1<<OCIE1A); // enable the timer1 output compare A interrupt
		105	coldstart = 1;
		106	}
		107
		108	void servoSetDefaultPos(void)
		109	{
		110	servoSetPosition(SERVO_PAN, ServoSteps()); // Ausgangsstellung SERVO_PAN
		111	servoSetPosition(SERVO_TILT,ServoSteps()); // Ausgangsstellung SERVO_TILT
		112	}
		113
		114	uint16_t pw_us(uint16_t Steps)
		115	{
		116	return(Steps * ServoConst[Config.sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in �s
		117	}
		118
		119	uint16_t ServoSteps(void)
		120	{
		121	return(ServoConst[Config.sIdxSteps].steps);
		122	}
		123
		124	void servoSet_rev(uint8_t channel, uint8_t val)
		125	{
		126	ServoChannels[channel].rev = val & 0x01;
		127	}
		128
		129	void servoSet_min(uint8_t channel, uint16_t min)
		130	{
		131	ServoChannels[channel].min = ServoConst[Config.sIdxSteps].min + min;
		132	}
		133
		134	void servoSet_max(uint8_t channel, uint16_t max)
		135	{
		136	ServoChannels[channel].max = ServoConst[Config.sIdxSteps].max - max;
		137	}
		138
		139	void servoSet_mid(uint8_t channel, uint16_t mid)
		140	{
		141	ServoChannels[channel].mid = mid;
		142	// Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen
		143	ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \
		144	(16 * mid - 8 * ServoConst[Config.sIdxSteps].steps))/16 + ServoChannels[channel].min;
		145	}
		146
		147	//! turns off software PWM system
		148	void servoOff(void)
		149	{
		150	// disable the timer1 output compare A interrupt
		151	TIMSK1 &= ~(1<<OCIE1A);
		152	}
		153
		154	//! set servo position on channel (raw unscaled format)
		155	void servoSetPositionRaw(uint8_t channel, uint16_t position)
		156	{
		157	// bind to limits
		158	if (position < ServoChannels[channel].min) position = ServoChannels[channel].min;
		159	if (position > ServoChannels[channel].max) position = ServoChannels[channel].max;
		160	// set position
		161	ServoChannels[channel].duty = position;
		162	}
		163
		164	//! set servo position on channel
		165	// input should be between 0 and ServoSteps() (entspricht Maximalausschlag)
		166	void servoSetPosition(uint8_t channel, uint16_t position)
		167	{ uint16_t pos_scaled;
		168
		169	// calculate scaled position
		170	if (ServoChannels[channel].rev != 0) position = ServoConst[Config.sIdxSteps].steps - position;
		171	if (position < ServoConst[Config.sIdxSteps].steps/2)
		172	//bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2)
		173	pos_scaled = ServoChannels[channel].min + ((int32_t)position2(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \
		174	ServoConst[Config.sIdxSteps].steps;
		175	else
		176	//bei Position >= Servomittelstellung
		177	pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[Config.sIdxSteps].steps / 2) \
		178	* 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[Config.sIdxSteps].steps;
		179	// set position
		180	servoSetPositionRaw(channel, pos_scaled);
		181	}
		182
		183	// Umrechnung Winkel in Servoschritte
		184	void servoSetAngle(uint8_t servo_nr, int16_t angle)
		185	{
		186	servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[Config.sIdxSteps].steps / 180));
		187	}
		188
		189	//Interruptroutine
		190	ISR(TIMER1_COMPA_vect)
		191	{ static uint8_t ServoChannel;
		192	uint16_t nextTics;
		193	uint16_t OCValue; // schedule next interrupt
		194
		195	if(ServoChannel < SERVO_NUM_CHANNELS) {
		196	PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel
		197	}
		198	ServoChannel++; // next channel
		199	if(ServoChannel != SERVO_NUM_CHANNELS) {
		200	// loop to channel 0 if needed
		201	if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0;
		202	// turn on new channel
		203	PORTD \|= ServoChannels[ServoChannel].pin;
		204	// schedule turn off time
		205	nextTics = ServoChannels[ServoChannel].duty;
		206	}
		207	else {
		208	// ***we could save time by precalculating this
		209	// schedule end-of-period
		210	nextTics = ServoPeriodTics-ServoPosTics;
		211	}
		212	// read in current value of output compare register OCR1A
		213	OCValue = OCR1AL; // read low byte of OCR1A
		214	OCValue += (OCR1AH <<8); // read high byte of OCR1A
		215	OCValue += nextTics; // increment OCR1A value by nextTics
		216	// set future output compare time to this new value
		217	OCR1AH = OCValue >> 8; // write high byte
		218	OCR1AL = OCValue & 0x00FF; // write low byte
		219
		220	ServoPosTics += nextTics; // set our new tic position
		221	if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
		222	}
		223

Subversion Repositories Projects

(root)/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/ng_servo.c @ 1924 - Rev