// UTS 2000 - Fichier 'servo.ccp' Gestion des servo moteur et affichage de la fenêtre de débug;
// V3.7c - Le 01/10/2016
/*****************************************************************************/
/* Début SERVO MOTEUR                                                        */
/*****************************************************************************/
#include  <windows.h>
#include  <string.h>
#include  <stdlib.h>
#include  <stdio.h>
#include "Train_UTS.h"
#include  "serial_port.h"

#define _CRT_SECURE_NO_WARNINGS  // Limite les messages de warning dans Visual C++

// Déclaration des fonctions externes
//extern int get_line(FILE*, char [][CHAR_CONFIG_NB], int*);
extern void EasyTextOut(int , int , char* );
extern void EasyLine(int, int, int, int);
extern cSerialPort serialPort;


// Déclaration des variables externes
extern HDC       hMemDC;
extern HFONT     bmp_fonte;
extern HPEN      bmp_stylo_noir, bmp_stylo_gris128, bmp_stylo_gris192, bmp_stylo_blanc, bmp_stylo_vert, bmp_stylo_jaune, bmp_stylo_orange, bmp_stylo_gris128, bmp_stylo_gris192, bmp_stylo_gris220, bmp_stylo_gris230;
extern HBRUSH    bmp_pinceau_blanc, bmp_pinceau_jaune, bmp_pinceau_vert, bmp_pinceau_orange, bmp_pinceau_gris192, bmp_pinceau_gris128, bmp_pinceau_gris192, bmp_pinceau_gris220, bmp_pinceau_gris230;
extern int touche_char;
extern int screen_mode;
extern int timer_prg;

extern int vio_srv[NB_CARTE][NB_SERVO];
extern int vio_srv_res[NB_CARTE][NB_SERVO];
extern int vio_srv_cmd[NB_CARTE][NB_SERVO+NB_SERVO_VIT]; // Consigne + vitesse à la suite.
extern int x_mouse,y_mouse, screen_mode_retour;

/* Définition des objets servos

  12345
 +-----
1|[]SRV : Voyant allumé = servo positionné + 'SRV'
2|  X-Y : Numero du servo
3|texte : Texte libre
4|texte

1 / Pour un déplacement ponctuel dans un sens ou l'autre,  exemple : une orientation d'un objet tant que le bouton reste enfoncé, 
avec comme configuration, #1=-1 et  #2=-1 :
#1    : -1
#2    : -1
S+    : = [+] ou [-] = sens du déplacement
V1    : Variable du servo = 8000-8004, 8100-8104
V2    : -1
OPT   : temps de rotation (0-255 max=25secondes) en 1/10 de secondes
TEXTE : texte


2 / Pour plusieurs positions du servo  (#2) avec plusieurs boutons poussoirs, un BP par position,  exemple : pont tournant, 
avec comme configuration, #1=-1 et #2 <>-1 :
#1    : -1
#2    : (0-1023) Valeur à atteindre, si le bouton poussoir a été actionné
V1    : Variable du servo = 8000-8004, 8100-8104
V2    : -1
OPT   : temps de rotation (0-255 max=25secondes) en 1/10 de secondes
TEXTE : texte


3 / Pour un déplacement incrémentale à pas fixe (+-#1), à chaque action d'un bouton poussoir,  exemple : une orientation d'un objet, 
avec comme configuration, #1<>-1 et #2=-1 :
#1    : (0-1023) Valeur du pas
#2    : -1
S+    : = [+] ou [-] = sens du déplacement
V1    : Variable du servo = 8000-8004, 8100-8104
V2    : -1
OPT   : temps de rotation (0-255 max=25secondes) en 1/10 de secondes
TEXTE : texte


4 / Pour deux positions, avec un bouton poussoir change de position à chaque action,  exemple : une animation, 
avec comme configuration, #1<>-1 et #2<>-1 et V2=-1 :
#1    : Valeur bouton poussoir au départ (0-1023)
#2    : Valeur bouton poussoir autre position (0-1023)
V1    : Variable du servo = 8000-8004, 8100-8104
V2    : -1
OPT   : temps de rotation (0-255 max=25secondes) en 1/10 de secondes
TEXTE : texte


5 / Pour deux positions commandées par une variable ou une entrée numérique, exemple : une barrière de passage à niveau, 
configurer une équation dans 'config.txt' :
#tdv
[
   equ: l  var_5 =val srv_cmd_000 111        -> si var_5 = 1 le servo est en position 111 (de 0 à 1023)
   equ: l  var_5 =val srv_cmd_010 10            srv_cmd_010 = vitesse du servo n°0 (de 1 lent à 1023 rapide)
   equ: ln var_5 =val srv_cmd_000 222        -> si var_5 = 0 le servo est en position 222 (de 0 à 1023)
   equ: ln var_5 =val srv_cmd_010 10            srv_cmd_010 = vitesse du servo n°0 (de 1 lent à 1023 rapide)
]
La vitesse du servo est dans la variable "srv_cmd_(n° de servo +10)".
Si on remplace 'var_5' par 'in_1', on commande les postions d'un servo à partir de l'entrée n°1 (0/1).


AIDE :
V1:VAR_SERVO (8000-8305),  TXT:TEXTE LIBRE
  DEPL_BP:INCREME_BP:PLUSIEU_BP:2 POSIT_BP
#1:   -1 :PAS 0_1023:        -1:P0  0_1023
#2:   -1 :        -1:BUT 0_1023:P1  0_1023
S+:  +/- :      +/- :(<-SENS DE ROTATION)
OPT: 0-255 TEMPS ROTATION MAX EN 1/10S


Dans le programme :
Le nombre de position a ajouter à chaque cycle = 1+1024/TICK = (1024*0.1)/(0.08+0.015)  (*0.1 car la tempo est en 1/10 de seconde)
Si Temp = 0-255
Nombre de positions à ajouter à chaque cycle du Timer = 1+(1024*(0.08+0.015))/Tempo = 1+972/Tempo

Pour faciliter les calculs, dans "config_tco.txt" on enregistre la valeur : Nb de positions = ENT(1+972/Tempo)

On retrouve le temps de déplacement maximum = Tempo = 1+ENT(972/Nb de position1).
*/

// Sous-programme de mise à jour des valeurs des servo moteur en fonction des consignes et de la duréé de rotation
// Appelé à chaque interruption du timer principal (80 + 15 msec).
//
void srv_maj(void)
{
	int i,j;
	// vio_srv_cmd[][] = consigne du servo (0-1023), converti en (0-255) dans 'serial_port.cpp' .
	// vio_srv[][] = position envoyée au servo (0-1023).
	for(i=0;i<NB_CARTE;i++) {
		for(j=0;j<NB_SERVO;j++) { 
			if(vio_srv[i][j]<vio_srv_cmd[i][j]) { // servo ++
				vio_srv[i][j]=vio_srv[i][j] + vio_srv_cmd[i][j+NB_SERVO_VIT];	//j+NB_SERVO_VIT pour obtenir les valeurs de vitesse.
				if(vio_srv[i][j]>vio_srv_cmd[i][j]) vio_srv[i][j]=vio_srv_cmd[i][j]; // Pour éviter de dépasser la consigne.
			}
			if(vio_srv[i][j]>vio_srv_cmd[i][j]) { // servo --
				vio_srv[i][j]=vio_srv[i][j] - vio_srv_cmd[i][j+NB_SERVO_VIT];	//j+NB_SERVO_VIT pour obtenir les valeurs de vitesse.
				if(vio_srv[i][j]<vio_srv_cmd[i][j]) vio_srv[i][j]=vio_srv_cmd[i][j]; // Pour éviter de dépasser la consigne.
			}
		}
	}
}


// Affiche le tableau des servos à partir du menu : 'Debug > Servos'.
//
void srv_debug(int action)
{
	int i,y,srv_i,srv_j;
	char ch[25]; // Pour afficher les nombres

	static int dimx=75; // Dimension horizontal d'une cellule
	static int dimy=16; // Dimension vertical d'une cellule
	static int nblignes=NB_SERVO*4; // Nombre de ligne du tableau (sans le titre)	
	static int decaly=16; //Decalage en pixel de la premiére ligne vers les bas.
	static int decalx=0; //Decalage en pixel de la premiére ligne vers la droite
	float nb_secondes;

	SelectObject(hMemDC,bmp_fonte); SetBkMode(hMemDC,TRANSPARENT);
	SelectObject(hMemDC,bmp_pinceau_blanc); SelectObject(hMemDC,bmp_stylo_gris192);

	if(action==DEBUG_TOTAL_PAINT) {     // ACTION = REDESSINE TOUTE LA FENETRE.
		Rectangle(hMemDC,0,0,800,700);
		SelectObject(hMemDC,bmp_pinceau_gris192); SelectObject(hMemDC,bmp_stylo_gris192); Rectangle(hMemDC,800-15,15,800-2,2); // Rectangle gris pour fermer la fenêtre.
		SelectObject(hMemDC,bmp_stylo_noir); EasyLine(800-14,3,800-3,14 ); EasyLine(800-14,13,800-3,2); // Croix noire.
		SelectObject(hMemDC,bmp_stylo_noir);  EasyLine(800-2,1,800-2,16); EasyLine(800-16,15,800-1,15); // Trait noir.
		SelectObject(hMemDC,bmp_stylo_gris128); EasyLine(800-3,2,800-3,15); EasyLine(800-15,14,800-2,14); // Trait gris foncé.
		SelectObject(hMemDC,bmp_pinceau_blanc); SelectObject(hMemDC,bmp_stylo_gris192);
		SetTextColor(hMemDC,RGB(0,0,0));	// Texte en noir.
 		EasyTextOut(2,2,"AFFICHE LES VALEURS,DES PARAMÈTRES DES SERVO-MOTEURS"); EasyTextOut(800-140,2,"(QUITTER = ESC)");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+1*16,"Consigne = Valeur à atteindre de 0 à 1023 (Valeur centrale = 512),");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+2*16,"           est convertie entre 0 et 255 dans la trame RS232 pour le PIC.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+3*16,"Position = Position actuelle (réelle) du servo.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+4*16,"           Derniére valeur utilisée (nb de pas du servo pour 1 tic).");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+5*16,"Vitesse  = Nb de pas du servo pour 1 tic (Dernière valeur utilisée de 1 à 1023).");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+6*16,"Temps    = Temps de déplacement pour aller d'une extrémité à l'autre en secondes.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+7*16,"Srv.Résv = Servo résérvé.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+8*16,"Dans 'config.txt', section '#tdv'");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+9*16,"  Utiliser les variables : 'srv_cmd_0 + srv_cmd_10' = 'Consigne + Vitesse' servo n° 0.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+10*16,"  Le n° de variable de la vitesse = au n° du servo + 10.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+11*16,"  La position réelle du servo = 'srv_pos_xxx'.");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+12*16,"  Utiliser les commandes '=var' ou '=val' pour commander un servo (Donner aussi la vitesse).");
		EasyTextOut(2,2+decaly+(nblignes+1)*dimy+13*16,"  Utiliser la commande '=varx4' pour utiliser une entrée CAN pour commander un servo.");
		// Lignes verticales
		for (i=0;i<=7;i++) { EasyLine(i*dimx+decalx, decaly, i*dimx+decalx, (nblignes+1)*dimy+decaly); }
		// Lignes horizontales
		for (i=0;i<nblignes+2;i++) { EasyLine(decalx, dimy*i+decaly, 7*dimx+decalx, dimy*i+decaly); }
		SetTextColor(hMemDC,RGB(255,0,0)); // Texte en rouge.
		y=0;
		EasyTextOut(       5+decalx,2+decaly,"n°SERVO" ); 
		EasyTextOut(5+1*dimx+decalx,2+decaly,"Consigne"); 
		EasyTextOut(5+2*dimx+decalx,2+decaly,"Position");
		EasyTextOut(5+3*dimx+decalx,2+decaly,"Cons/Pos");
		EasyTextOut(8+4*dimx+decalx,2+decaly,"Vitesse");
		EasyTextOut(1+5*dimx+decalx,2+decaly,"  Temps");
		EasyTextOut(5+6*dimx+decalx,2+decaly,"Srv.Résv");
		SelectObject(hMemDC,bmp_pinceau_gris192); SelectObject(hMemDC,bmp_stylo_gris192);
		Rectangle(hMemDC,10,2+decaly+(nblignes+1)*dimy+2*16-2,65,2+decaly+(nblignes+1)*dimy+2*16+8); // Consigne
		SelectObject(hMemDC,bmp_pinceau_gris128); SelectObject(hMemDC,bmp_stylo_gris128);
		Rectangle(hMemDC,10,2+decaly+(nblignes+1)*dimy+4*16-2,65,2+decaly+(nblignes+1)*dimy+4*16+8); // Position
	}

	if(action==DEBUG_MOUSE) {	// ACTION = CLIC GAUCHE DE LA SOURIS
			if ((x_mouse > 800-15 && x_mouse < 800-2) && (y_mouse > 2 && y_mouse < 15)) {
			screen_mode= screen_mode_retour; 
			return;	// On ferme la fenêtre.
		}
	}


	if(action==DEBUG_MOUSE_RIGHT) { }    // ACTION = CLIC DROIT DE LA SOURIS.

	if(action==DEBUG_KEY) { }          // ACTION = TOUCHE ENFONCEE

	SetTextColor(hMemDC,RGB(0,0,255));	// Texte en bleu.
	for(i=0; i<NB_SERVO*NB_CARTE; i++) {

		y=dimy*i+decaly+dimy;
		SelectObject(hMemDC,bmp_pinceau_blanc);
		srv_j=(i/NB_SERVO); srv_i=(i%NB_SERVO);
		if(srv_j>=NB_CARTE) continue;
		if (srv_j==0 || (srv_j==1 && serialPort.card1_ok) || (srv_j==2 && serialPort.card2_ok) || (srv_j==3 && serialPort.card3_ok)) {
			 SetTextColor(hMemDC,RGB(0,0,255)); // En bleu si carte n°0 ou si connectée.
		}
		else  { SetTextColor(hMemDC,RGB(192,192,192)); } // En gris si carte <> n°0 et absente.
		sprintf_s(ch,"%4d",100*srv_j+srv_i);				    EasyTextOut(      15+decalx,y+2,ch); // N° de servo
		SelectObject(hMemDC,bmp_fonte); SetBkMode(hMemDC,TRANSPARENT);// SetTextColor(hMemDC,RGB(0,0,255));
		Rectangle(hMemDC,decalx+1*dimx,decaly+(i+1)*dimy,decalx+2*dimx+1,decaly+(i+2)*dimy+1);
		sprintf_s(ch,"%6d",vio_srv_cmd[srv_j][srv_i]);          EasyTextOut(2+decalx+1*dimx,y+2,ch); // Consigne (but)
		Rectangle(hMemDC,decalx+2*dimx,decaly+(i+1)*dimy,decalx+3*dimx+1,decaly+(i+2)*dimy+1);
		sprintf_s(ch,"%6d",vio_srv[srv_j][srv_i]);			    EasyTextOut(2+decalx+2*dimx,y+2,ch); // Position actuelle du servo
		Rectangle(hMemDC, decalx+3*dimx, decaly+(i+1)*dimy, decalx+4*dimx+1, decaly+(i+2)*dimy+1);


		if (srv_j == 0 || (srv_j == 1 && serialPort.card1_ok) || (srv_j == 2 && serialPort.card2_ok) || (srv_j == 3 && serialPort.card3_ok)) {
			SelectObject(hMemDC, bmp_pinceau_gris192); SelectObject(hMemDC, bmp_stylo_gris192);
		}// Consigne = Gris clair
		else SelectObject(hMemDC, bmp_pinceau_gris230); SelectObject(hMemDC, bmp_stylo_gris230); // Consigne = Gris plus clair
		if (vio_srv_cmd[srv_j][srv_i]>18) Rectangle(hMemDC, decalx+3*dimx+1, decaly+(i+1)*dimy+1+1, decalx+3*dimx+((vio_srv_cmd[srv_j][srv_i]*(dimx))/1023), decaly+(i+2)*dimy-7);



		if (srv_j == 0 || (srv_j == 1 && serialPort.card1_ok) || (srv_j == 2 && serialPort.card2_ok) || (srv_j == 3 && serialPort.card3_ok)) {
			SelectObject(hMemDC, bmp_pinceau_gris128); SelectObject(hMemDC, bmp_stylo_gris128);
		}// Consigne = Gris clair
		else SelectObject(hMemDC,bmp_pinceau_gris220); SelectObject(hMemDC,bmp_stylo_gris220); // Position = Gris foncé
		if (vio_srv[srv_j][srv_i]>18) Rectangle(hMemDC, decalx+3*dimx+1, decaly+(i+1)*dimy+9, decalx+3*dimx+((vio_srv[srv_j][srv_i]*(dimx))/1023), decaly+(i+2)*dimy-1);


		SelectObject(hMemDC,bmp_pinceau_blanc); SelectObject(hMemDC,bmp_stylo_gris192);
		Rectangle(hMemDC, decalx+4*dimx,decaly+(i+1)*dimy, decalx+5*dimx+1,decaly+(i+2)*dimy+1);
		if (vio_srv_cmd[srv_j][srv_i+NB_SERVO_VIT]!=0) {
			sprintf_s(ch,"%6d",vio_srv_cmd[srv_j][srv_i+NB_SERVO_VIT]); EasyTextOut(2+decalx+4*dimx,y+2,ch); // Vitesse de déplacement
		}
		Rectangle(hMemDC,decalx+5*dimx,decaly+(i+1)*dimy,decalx+6*dimx+1,decaly+(i+2)*dimy+1);
		if (vio_srv_cmd[srv_j][srv_i+NB_SERVO_VIT]!=0) {
			nb_secondes=1023*(timer_prg+15);
			nb_secondes=nb_secondes/((vio_srv_cmd[srv_j][srv_i+NB_SERVO_VIT])*1000);
			sprintf_s(ch,"%4.1f",nb_secondes); EasyTextOut(2+decalx+5*dimx+20,y+2,ch); // Temps de déplacement
		}
		else EasyTextOut(2+decalx+5*dimx+35,y+2,"-"); // Temps de déplacement infini
		Rectangle(hMemDC,decalx+6*dimx,decaly+(i+1)*dimy,decalx+7*dimx+1,decaly+(i+2)*dimy+1);
		if (vio_srv_res[srv_j][srv_i]!=0) {
			sprintf_s(ch,"%6d",vio_srv_res[srv_j][srv_i]);          EasyTextOut(2+decalx+6*dimx-10,y+2,ch); // Servo réservé
		}
	}
}
/*****************************************************************************/
/* fin SERVO MOTEURS                                                         */
/*****************************************************************************/
