/*
 * Carte de gestion de 12 servomoteurs sur bus CAN
 *
 * Christophe Le Lann <contact@totofweb.net>
 * http://www.totofweb.net/robots-projet-66.html
 *
 * La méthode de génération est basée sur un tableau de masques
 * binaires ordonnés dans le temps :
 *  - Au début de la génération des signaux, on active à l'état
 *    haut toutes les sorties de signaux.
 *  - Petit à petit, on remet à l'état bas les signaux les uns
 *    après les autres grâce à des masques binaires, déjà ordonnés
 *    par ordre chronologique
 *  - Pendant le temps libre entre deux signaux, on génère les
 *    prochains masques binaires à partir des nouvelles consignes
 *    de position et des paramètres de vitesse
 *
 * Avec cette méthode, on obtient des timings très précis tout en
 * minimisant l'occupation du microcontrôleur, qui n'est occupé
 * par la génération des signaux que pendant 2.5ms toutes les 20ms.
 */

//-----------------------------
// Librairies
//-----------------------------

#include <avr/io.h>				// Mapping des registres   - http://www.nongnu.org/avr-libc/user-manual/group__avr__io.html
#include <avr/eeprom.h>			// Lecture/écriture EEPROM - http://www.nongnu.org/avr-libc/user-manual/group__avr__eeprom.html
#include <util/atomic.h>		// Garantie d'atomicité    - http://www.nongnu.org/avr-libc/user-manual/group__util__atomic.html

//-----------------------------
// Librairies persos
//-----------------------------

#include "../drivers/timer0.h"
#include "../drivers/timer1.h"
#include "servos.h"

//-----------------------------
// Variables globales
//-----------------------------

volatile unsigned char servos_newsig_flag = 0;		// Flag signalant la fin des 20ms depuis le début de la génération des signaux
srv_t servos[SERVOS_NB];
unsigned int  servos_enabled;						// servo N activé si bit N à 1
srv_masque_t servos_masques[SERVOS_NB];

// Stockage statique en EEPROM
EEMEM struct {
	unsigned char target;
	unsigned char vit;
} servos_eeprom[SERVOS_NB];

//-----------------------------
// Fonctions
//-----------------------------

void servos_init(void) {				// Initialisation des timers
	unsigned char i;

	// Initialisation à l'état bas
	SRV_00_TO_07_PORT = 0;
	SRV_00_TO_07_DDR = ~0;
	SRV_08_TO_11_PORT &= ~(0x0F << SRV_08_TO_11_SHIFT);
	SRV_08_TO_11_DDR |= (0x0F << SRV_08_TO_11_SHIFT);

	// Configuration du Timer0 en CTC à 100Hz pour la période globale des signaux
	timer0ctc_init(100);
	timer0ctc_isr_enable(servos_newsig);

	// Configuration du Timer1 à 250kHz pour le temps interne à la génération de signaux
	timer1_setmode(T1_MODE_NORMAL);
	timer1_setpresc(T1_CLK_DIV64);

	// Par défaut, tous les servos sont désactivés, en vitesse maximale
	servos_enabled = 0;
	for (i = 0; i < SERVOS_NB; i++) {
		servos[i].pos = 127;
		servos[i].target = 127;
		servos[i].vit = 127;
		servos[i].delayeval = 0;
	}
}
void servos_eepromsave(void) {			// Sauvegarde des paramètres actuels en EEPROM
	unsigned char i;

	// Désactivation obligatoires des interruptions, sinon boom
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		for (i = 0; i < SERVOS_NB; i++) {
			eeprom_busy_wait();
			eeprom_write_byte(&servos_eeprom[i].target, servos[i].target);
			eeprom_busy_wait();
			eeprom_write_byte(&servos_eeprom[i].vit, servos[i].vit);
		}
	}
}
void servos_eepromrestore(void) {		// Restauration des paramètres enregistrés en EEPROM
	unsigned char i;

	// Désactivation obligatoires des interruptions, sinon boom
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		for (i = 0; i < SERVOS_NB; i++) {
			eeprom_busy_wait();
			servos[i].target = eeprom_read_byte(&servos_eeprom[i].target);
			eeprom_busy_wait();
			servos[i].vit = eeprom_read_byte(&servos_eeprom[i].vit);
			servos[i].pos = servos[i].target;
			servos[i].delayeval = 0;
		}
	}
}
void servos_waitnewsig(void) {			// Attente du prochain début de signaux (toutes les 20ms)
	// On attend le début de la période de 20ms
	while (!servos_newsig_flag);
	servos_newsig_flag = 0;
}
void servos_genersig(void) {			// Génération des signaux PWM (durée : 2.5ms)
	// On suppose que les couples (masque, tempo) sont prêts
	register unsigned char i;
	unsigned int tempo;

	// Début des signaux
	SRV_00_TO_07_PORT |= (unsigned char)(servos_enabled & 0xFF);
	SRV_08_TO_11_PORT |= (unsigned char)(((servos_enabled >> 8) & 0xFF) << SRV_08_TO_11_SHIFT);

	// On commence par attendre 400us
	// Le Timer1 est incrémenté toutes les 4µs
	timer1_cleartcnt();
	while (timer1_gettcnt() < 100);
	timer1_cleartcnt();

	// Extinction des signaux les uns après les autres, dans l'ordre
	// les servos_masques[i].tempo représentent une partition des temps 0.4 à 2.4ms en 256 valeurs
	for (i = 0; i < SERVOS_NB; i++) {
		tempo = ((unsigned int)(servos_masques[i].tempo) << 1);
		while (timer1_gettcnt() < tempo);
		SRV_00_TO_07_PORT &= servos_masques[i].masque_low;
		SRV_08_TO_11_PORT &= servos_masques[i].masque_high;
		if (tempo == 255) return;
	}
}
void servos_prepare(void) {				// Préparation des listes et masques qui serviront à traiter rapidement la génération des signaux
	unsigned int servos_tries = ~servos_enabled;
	register unsigned char i;
	unsigned char srv_prochain_id;
	unsigned char srv_prochain_tempo;
	unsigned char masque_id = 255;

	// On actualise les commandes des servos en fonction des paramètres de vitesse demandés
	for (i = 0; i < SERVOS_NB; i++) {
		if (servos[i].pos != servos[i].target) {
			if (servos[i].vit < 128) {
				// Mode vitesse rapide : vit est le delta max de "pos" entre deux signaux
				if (servos[i].pos < servos[i].target) {
					if (servos[i].target - servos[i].pos > servos[i].vit)
						servos[i].pos += servos[i].vit;
					else
						servos[i].pos = servos[i].target;
				} else {
					if (servos[i].pos - servos[i].target > servos[i].vit)
						servos[i].pos -= servos[i].vit;
					else
						servos[i].pos = servos[i].target;
				}
			} else {
				// Mode vitesse lente : (vit-128) est le nombre de signaux entre deux variations de "pos"
				if (servos[i].delayeval == 0) {
					if (servos[i].pos < servos[i].target)
						servos[i].pos++;
					else
						servos[i].pos--;
					servos[i].delayeval = servos[i].vit - 128;
				} else {
					servos[i].delayeval--;
				}
			}
		}
	}

	// Initialisation des couples (masque, tempo)
	for (i = 0; i < SERVOS_NB; i++) {
		servos_masques[i].tempo = 255;
		servos_masques[i].masque_low = ~0;
		servos_masques[i].masque_high = ~0;
	}

	while (servos_tries != ~((unsigned int)0)) {
		// On recherche le prochain servo à désactiver (plus petite tempo)
		srv_prochain_tempo = 255;
		srv_prochain_id = 12;
		for (i = 0; i < SERVOS_NB; i++) {
			if (!(servos_tries & (1 << i)) && (servos_enabled & (1 << i)) && servos[i].pos <= srv_prochain_tempo) {
				srv_prochain_id = i;
				srv_prochain_tempo = servos[i].pos;
			}
		}
		if (srv_prochain_id == 12) {
			// Erreur, on était censé en trouver au moins un !
			return;
		}

		// Soit la tempo est la même que pour le précédent (on l'inclus dans le masque du précédent),
		// soit on crée un nouveau couple (masque, tempo)
		if (masque_id == 255 || srv_prochain_tempo > servos_masques[masque_id].tempo)
			masque_id++;

		servos_masques[masque_id].tempo = srv_prochain_tempo;
		servos_masques[masque_id].masque_low &= ~(1 << srv_prochain_id);
		servos_masques[masque_id].masque_high &= ~(1 << (srv_prochain_id - 8 + SRV_08_TO_11_SHIFT));

		servos_tries |= (1 << srv_prochain_id);
	}
}

//-----------------------------
// Interruptions
//-----------------------------

void servos_newsig(void) {
	static unsigned char altern;	// Flag permettant de dédoubler virtuellement la fréquence de l'interruption
	altern++;
	if (altern % 2)
		servos_newsig_flag = 1;
}