WebSVN - Projects - Blame - /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/ng

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

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/ng_servo.c @ 2208 - Rev