// This file is identical to servoext.c, except that it only has absolute servo position functions.:

// With a 20Mhz crystal, and T1_DIV_8, T1 interupts every (4*8*(2^16-53660))/20000000 = 19ms < 20ms
#define TIMER_VAL	53660

// Specifics of the servos
#define SHORT_TIME	0.0010
#define	CENTRE_TIME	0.0015
#define LONG_TIME	0.0020
#define SCALE		(LONG_TIME-SHORT_TIME)/255.0

// Angles of servos. timerangle is the time duration of timer3 corresponding to the pulsewidth required for the angle.
int8 angle[] = {0,0,0,0,0,0,0,0,0};
int16 timerangle[] = {0,0,0,0,0,0,0,0,0};

// Pin description of the servos. number of servos. Maximum of 9. Might be able to squeeze a few more in.
int16 servo[] = {0,0,0,0,0,0,0,0,0};
int8 num = 0;

// Keeps track of the servo we are currently dealing with.
int8 cur;

// Turn an angle into a timer length. Angle 0 is full left, angle 255 is full right.
int16 calc_tangle(int8 a) {
	return 0xFFFF- (int16)((a*SCALE + SHORT_TIME)*1250000);
}

// Input the pin descriptions of the servos, eg. int16 servos[] = {PIN_B6, PIN_C5, PIN_B2}; init_servoabs(servos, 3);
void init_servoabs(int16 s[], int8 n) {
	int8 i;

	num = n;
	cur = 0;

	for (i = 0; i < n; i++) {
		if (s[i] != 0) {
			servo[i] = s[i];
		}
	}

	for (i = 0; i < num; i++) {
		angle[i] = 127;
		timerangle[i] = calc_tangle(angle[i]);
	}

	setup_timer_1(T1_DIV_BY_8 | T1_INTERNAL);
	set_timer1(TIMER_VAL);
	
	setup_timer_3(T3_DIV_BY_4 | T3_INTERNAL);

	enable_interrupts(INT_TIMER1);
	disable_interrupts(INT_TIMER3);
	enable_interrupts(GLOBAL);

}

// Set the angle of servo i. Angle 0 is full left, angle 255 is full right.
void set_angle(int8 ang, int8 n) {
	angle[n] = ang;
	timerangle[n] = calc_tangle(ang);
}

int8 get_angle(int8 i) {
	return angle[i];
}

#int_timer1
void isr_timer1() {
	cur = 0;
	output_high(servo[cur]);
	set_timer3(timerangle[cur]);
	enable_interrupts(int_timer3);

	clear_interrupt(INT_TIMER1);
	set_timer1(TIMER_VAL);
}

#int_timer3
void isr_timer3() {
	output_low(servo[cur]);

	cur++;
	if (cur == num) {
		disable_interrupts(INT_TIMER3);
		clear_interrupt(INT_TIMER3);
	} else {
		output_high(servo[cur]);
		clear_interrupt(INT_TIMER3);
		set_timer3(timerangle[cur]);
	}

}