Herkulex_Library_2021_phare - Lib herkulex utilisée pour la carte du phare

Users » thesnowfoxy » Code » Herkulex_Library_2021_phare

Sébastien Posada Ramirez / Herkulex_Library_2021_phare

Lib herkulex utilisée pour la carte du phare

fonctions_herkulex.cpp@16:fe49d838a0cc, 2020-07-09 (annotated)

Committer:: maximilienlv
Date:: Thu Jul 09 18:23:34 2020 +0000
Revision:: 16:fe49d838a0cc
Parent:: 14:a6e5d1ce2133
Child:: 17:d8f186634748

ajout des capteurs de couleurs herkulex (pas encore optimise)

Who changed what in which revision?

User	Revision	Line number	New contents of line
AdrienSalle	0:32fd5a08c430	1	#include "mbed.h"
AdrienSalle	0:32fd5a08c430	2	#include "fonctions_herkulex.h"
AdrienSalle	0:32fd5a08c430	3	//#include "ident_crac.h"
AdrienSalle	0:32fd5a08c430	4
AdrienSalle	0:32fd5a08c430	5
AdrienSalle	0:32fd5a08c430	6	//-------------------------Définition des ports série---------------------------
kyxstark	6:81733a7b69e9	7	//RawSerial pc(USBTX, USBRX,115200);
AdrienSalle	0:32fd5a08c430	8
maximilienlv	14:a6e5d1ce2133	9	RawSerial serial1(PB_6,PB_7,115200); // P : 41 et 42
maximilienlv	14:a6e5d1ce2133	10	RawSerial serial2(PC_12,PD_2,115200); // P : 43 et 47
maximilienlv	14:a6e5d1ce2133	11	RawSerial serial3(PC_10,PC_11,115200); // P : 44 et 48
maximilienlv	14:a6e5d1ce2133	12	RawSerial serial4(PC_6,PC_7,115200); // P : 45
maximilienlv	14:a6e5d1ce2133	13	RawSerial serial5(PA_0,PA_1,115200); // P : 46
AdrienSalle	0:32fd5a08c430	14
AdrienSalle	0:32fd5a08c430	15
AdrienSalle	0:32fd5a08c430	16	//----------------------------variables de reception----------------------------
AdrienSalle	0:32fd5a08c430	17	uint8_t rx[300];
AdrienSalle	0:32fd5a08c430	18	uint8_t rx2[256];
AdrienSalle	0:32fd5a08c430	19	unsigned char size_reponse=100;
AdrienSalle	0:32fd5a08c430	20	unsigned char recevoir = 0;
AdrienSalle	0:32fd5a08c430	21	unsigned char i2 = 0;
AdrienSalle	0:32fd5a08c430	22	unsigned char flag_serial1_receive2 = 0;
AdrienSalle	0:32fd5a08c430	23	//--------------------variables et fonction de verification---------------------
AdrienSalle	0:32fd5a08c430	24	#define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
AdrienSalle	0:32fd5a08c430	25	#define tolerance_en_position_negatif -16
AdrienSalle	0:32fd5a08c430	26	#define B_tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
AdrienSalle	0:32fd5a08c430	27	#define B_tolerance_en_position_negatif -16
AdrienSalle	0:32fd5a08c430	28	#define V_b 45 //temps d'attente de bras
AdrienSalle	0:32fd5a08c430	29	#define V_m 45 //temps d'attente de bras
AdrienSalle	0:32fd5a08c430	30	#define V_h 45 //temps d'attente de bras
AdrienSalle	0:32fd5a08c430	31	#define TEMPO_R 16 //temps d'attente de reception
AdrienSalle	0:32fd5a08c430	32	#define PWM_recl 0.6
AdrienSalle	0:32fd5a08c430	33	#define temps_servo_G 65 //temps d'attente des servos sauf 2 bras
AdrienSalle	0:32fd5a08c430	34	#define new_tempoX 45 //temps d'attente de correction double
AdrienSalle	0:32fd5a08c430	35
AdrienSalle	0:32fd5a08c430	36
AdrienSalle	0:32fd5a08c430	37	int16_t pos_position = 0, get_pos = 0, pos_ID = 0;
AdrienSalle	0:32fd5a08c430	38	uint8_t pos_led = 0, Status = 0,iID = 0;
AdrienSalle	0:32fd5a08c430	39	uint8_t nombre_servo = 0;
AdrienSalle	0:32fd5a08c430	40	uint8_t pos_time = 0;
AdrienSalle	0:32fd5a08c430	41	uint16_t position_servo_mul[20];
AdrienSalle	0:32fd5a08c430	42	uint8_t data_servo_mul[40];
AdrienSalle	0:32fd5a08c430	43	uint8_t flag_correction = 0;
AdrienSalle	0:32fd5a08c430	44	float new_tempo=0;
AdrienSalle	0:32fd5a08c430	45	float tab_tempo[20];
AdrienSalle	0:32fd5a08c430	46	uint16_t position_servo_mul_different[20];
AdrienSalle	0:32fd5a08c430	47	uint8_t data_servo_mul_different[60];
AdrienSalle	0:32fd5a08c430	48	int8_t my_Tor = 0;
AdrienSalle	0:32fd5a08c430	49	int8_t Tension_inter = 0;
AdrienSalle	0:32fd5a08c430	50	double Tension = 0;
AdrienSalle	0:32fd5a08c430	51	uint8_t coeffient_time = 1;
AdrienSalle	0:32fd5a08c430	52	uint8_t veri = 0;
AdrienSalle	0:32fd5a08c430	53	typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ;
AdrienSalle	0:32fd5a08c430	54	unsigned char serial_numero = 0;
AdrienSalle	0:32fd5a08c430	55	static type_etat etat=pos;
AdrienSalle	0:32fd5a08c430	56	int nb_erreur_pas_de_couple = 0;
AdrienSalle	0:32fd5a08c430	57	int nb_erreur_pos = 0;
AdrienSalle	0:32fd5a08c430	58
AdrienSalle	0:32fd5a08c430	59
AdrienSalle	0:32fd5a08c430	60
AdrienSalle	0:32fd5a08c430	61	//---------------------fonction d'interruption de reception de serial1---------------------
AdrienSalle	0:32fd5a08c430	62	/*
AdrienSalle	0:32fd5a08c430	63	*Ici on crée une interruption afin de rendre prioritaire la réception de données
AdrienSalle	0:32fd5a08c430	64	*!!! Pour utiliser les fonctions utilisant les interruptions, il faut 'activer' ces dernières
AdrienSalle	0:32fd5a08c430	65	*!!!avec la fonction servo_interrupt_en();
AdrienSalle	0:32fd5a08c430	66	*/
AdrienSalle	0:32fd5a08c430	67	unsigned char flag_perdu_info_serial1 = 0, indicateur_serial1 = 0, Size_trame_serial1 = 0, old_valueserial1 = 0;
AdrienSalle	0:32fd5a08c430	68	unsigned char char_receive_pc[100];
AdrienSalle	0:32fd5a08c430	69	unsigned char char_receive_serial1[100];
AdrienSalle	0:32fd5a08c430	70	unsigned char valueserial1=0;
AdrienSalle	0:32fd5a08c430	71	unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	72	int pospos;
Artiom	11:542de77801ba	73
AdrienSalle	0:32fd5a08c430	74	//
AdrienSalle	0:32fd5a08c430	75	//La fonction receive_serial1() est appelée par la fonction d'interruption
AdrienSalle	0:32fd5a08c430	76	//elle appelle la fonction automate_serial() et range les données reçues dans un tableau
AdrienSalle	0:32fd5a08c430	77	//
AdrienSalle	0:32fd5a08c430	78	void receive_serial1()
AdrienSalle	0:32fd5a08c430	79	{
AdrienSalle	0:32fd5a08c430	80	char_receive_serial1[valueserial1]=serial1.getc();
AdrienSalle	0:32fd5a08c430	81	automate_serial1();
AdrienSalle	0:32fd5a08c430	82	}
AdrienSalle	0:32fd5a08c430	83	//
AdrienSalle	0:32fd5a08c430	84	//fonction d'interruption
AdrienSalle	0:32fd5a08c430	85	// elle se déclenche dès que des données se trouvent sur le port de réception
AdrienSalle	0:32fd5a08c430	86	// elle appelle alors immédiatement la fonction receive_serial1();
AdrienSalle	0:32fd5a08c430	87	//
AdrienSalle	0:32fd5a08c430	88	void Interrupt1_en(void)
AdrienSalle	0:32fd5a08c430	89	{
AdrienSalle	0:32fd5a08c430	90	serial1.attach(&receive_serial1,Serial::RxIrq);
AdrienSalle	0:32fd5a08c430	91	}
Artiom	11:542de77801ba	92	bool verification()
Artiom	5:3de53c9af201	93	{
Artiom	5:3de53c9af201	94	uint8_t i = 0;
Artiom	5:3de53c9af201	95	switch(etat) {
Artiom	5:3de53c9af201	96	case pos:
Artiom	5:3de53c9af201	97	//------------------------Status--------------------
Artiom	5:3de53c9af201	98	Status = getStatus(pos_ID,serial_numero);
Artiom	5:3de53c9af201	99	wait_ms(3);
kyxstark	6:81733a7b69e9	100	////pc.printf("status = %d",Status);
Artiom	5:3de53c9af201	101	switch(Status) {
Artiom	5:3de53c9af201	102	case 0:
Artiom	5:3de53c9af201	103	break;
Artiom	5:3de53c9af201	104
Artiom	5:3de53c9af201	105	case 2: //Exceed allowed POT limit
kyxstark	6:81733a7b69e9	106	//pc.printf("ERR-Depasse la limite de position\n");
Artiom	5:3de53c9af201	107	//clean_ERR(pos_ID);
Artiom	5:3de53c9af201	108	//wait_ms(500);
Artiom	5:3de53c9af201	109	clear(pos_ID,serial_numero);
Artiom	5:3de53c9af201	110	//positionControl(pos_ID, 1000, 3, GLED_ON);
Artiom	5:3de53c9af201	111	wait_ms(3);
Artiom	5:3de53c9af201	112	Status = getStatus(pos_ID,serial_numero);
Artiom	5:3de53c9af201	113	wait_ms(3);
kyxstark	6:81733a7b69e9	114	////pc.printf("status = %d",Status);
Artiom	5:3de53c9af201	115	break;
Artiom	5:3de53c9af201	116	}
Artiom	5:3de53c9af201	117	//------------------Torque et position------------------------------
Artiom	5:3de53c9af201	118	my_Tor = Get_Torque(pos_ID,serial_numero);
Artiom	5:3de53c9af201	119	wait_ms(5);
kyxstark	6:81733a7b69e9	120	////pc.printf("my_Tor = %x\n",my_Tor);
Artiom	5:3de53c9af201	121	while(my_Tor != 0x60) {
Artiom	5:3de53c9af201	122	setTorque(pos_ID,TORQUE_ON,serial_numero);
Artiom	5:3de53c9af201	123	my_Tor = Get_Torque(pos_ID,serial_numero);
Artiom	5:3de53c9af201	124	wait_ms(5);
Artiom	5:3de53c9af201	125	}
Artiom	5:3de53c9af201	126	Tension_inter = Get_Tension_actuelle(pos_ID,serial_numero);
Artiom	5:3de53c9af201	127	Tension = Tension_inter*0.074;
Artiom	5:3de53c9af201	128	if(Tension <=6.60) {
Artiom	5:3de53c9af201	129	coeffient_time = 6;
Artiom	5:3de53c9af201	130	} else if(Tension <= 6.90) {
Artiom	5:3de53c9af201	131	coeffient_time = 4;
Artiom	5:3de53c9af201	132	} else if(Tension <= 7.10) {
Artiom	5:3de53c9af201	133	coeffient_time = 2;
Artiom	5:3de53c9af201	134	} else if(Tension > 7.10) {
Artiom	5:3de53c9af201	135	coeffient_time = 1;
Artiom	5:3de53c9af201	136	}
Artiom	5:3de53c9af201	137	get_pos = getPos(pos_ID,serial_numero);
kyxstark	6:81733a7b69e9	138	//pc.printf("P4=%d ",get_pos);
Artiom	5:3de53c9af201	139	if(((get_pos - pos_position)>tolerance_en_position)\|\|((get_pos - pos_position)<tolerance_en_position_negatif)) {
Artiom	5:3de53c9af201	140	if((get_pos - pos_position)>tolerance_en_position) {
Artiom	5:3de53c9af201	141	new_tempo=(get_pos - pos_position)0.084coeffient_time + 1;
Artiom	5:3de53c9af201	142	if (new_tempo > 254) new_tempo = 254;
Artiom	5:3de53c9af201	143	} else if((get_pos - pos_position)<tolerance_en_position_negatif) {
Artiom	5:3de53c9af201	144	new_tempo=(get_pos - pos_position)0.084coeffient_time +1;
Artiom	5:3de53c9af201	145	if (new_tempo > 254) new_tempo = 254;
Artiom	5:3de53c9af201	146	}
Artiom	5:3de53c9af201	147	positionControl(pos_ID, pos_position, new_tempo, pos_led,serial_numero);
kyxstark	6:81733a7b69e9	148	//pc.printf("Correction!\n");
Artiom	5:3de53c9af201	149	}
Artiom	5:3de53c9af201	150	break;
Artiom	5:3de53c9af201	151	case pos_mul_complex:
Artiom	5:3de53c9af201	152	//---------------------------Status---------------------------
Artiom	5:3de53c9af201	153	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	154	Status = getStatus(data_servo_mul[1+2*i],serial_numero);
kyxstark	6:81733a7b69e9	155	//pc.printf("status = %d",Status);
Artiom	5:3de53c9af201	156	switch(Status) {
Artiom	5:3de53c9af201	157	case 0:
Artiom	5:3de53c9af201	158	break;
Artiom	5:3de53c9af201	159
Artiom	5:3de53c9af201	160	case 2: //Exceed allowed POT limit
kyxstark	6:81733a7b69e9	161	////pc.printf("ERR-Depasse la limite de position\n");
Artiom	5:3de53c9af201	162	//clean_ERR(id);
Artiom	5:3de53c9af201	163	//wait_ms(500);
Artiom	5:3de53c9af201	164	clear(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	165	//positionControl(id, 1000, 3, GLED_ON);
Artiom	5:3de53c9af201	166	//wait_ms(3);
Artiom	5:3de53c9af201	167	//Status = getStatus(data_servo_mul[1+2*i]);
Artiom	5:3de53c9af201	168	//wait_ms(3);
kyxstark	6:81733a7b69e9	169	////pc.printf("status = %d",Status);
Artiom	5:3de53c9af201	170	break;
Artiom	5:3de53c9af201	171	}
Artiom	5:3de53c9af201	172	}
Artiom	5:3de53c9af201	173	//----------------------Torque et position--------------------------
Artiom	5:3de53c9af201	174	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	175	my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	176	while(my_Tor != 0x60) {
Artiom	5:3de53c9af201	177	setTorque(data_servo_mul[1+2*i],TORQUE_ON,serial_numero);
Artiom	5:3de53c9af201	178	my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
kyxstark	6:81733a7b69e9	179	////pc.printf(" SET_TORQUE ");
Artiom	5:3de53c9af201	180
Artiom	5:3de53c9af201	181	Status = getStatus(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	182	clear(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	183	Status = getStatus(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	184	}
Artiom	5:3de53c9af201	185	}
Artiom	5:3de53c9af201	186	veri = 0;
Artiom	5:3de53c9af201	187	while(veri < nombre_servo) {
Artiom	5:3de53c9af201	188	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	189	my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	190	while(my_Tor != 0x60) {
Artiom	5:3de53c9af201	191	setTorque(data_servo_mul[1+2*i],TORQUE_ON,serial_numero);
Artiom	5:3de53c9af201	192	my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero);
kyxstark	6:81733a7b69e9	193	////pc.printf(" SET_TORQUE ");
Artiom	5:3de53c9af201	194
Artiom	5:3de53c9af201	195	Status = getStatus(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	196	clear(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	197	Status = getStatus(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	198	}
Artiom	5:3de53c9af201	199	}
Artiom	5:3de53c9af201	200	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	201	Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i],serial_numero);
Artiom	5:3de53c9af201	202	Tension = Tension_inter*0.074;
Artiom	5:3de53c9af201	203	if(Tension <=6.60) {
Artiom	5:3de53c9af201	204	coeffient_time = 6;
Artiom	5:3de53c9af201	205	} else if(Tension <= 6.90) {
Artiom	5:3de53c9af201	206	coeffient_time = 4;
Artiom	5:3de53c9af201	207	} else if(Tension <= 7.10) {
Artiom	5:3de53c9af201	208	coeffient_time = 2;
Artiom	5:3de53c9af201	209	} else if(Tension > 7.10) {
Artiom	5:3de53c9af201	210	coeffient_time = 1;
Artiom	5:3de53c9af201	211	}
Artiom	5:3de53c9af201	212	get_pos = getPos(data_servo_mul[1+2*i],serial_numero);
kyxstark	6:81733a7b69e9	213	//pc.printf("PosiM=%d ",get_pos);
Artiom	5:3de53c9af201	214	if((get_pos - position_servo_mul[i])>tolerance_en_position) {
Artiom	5:3de53c9af201	215	tab_tempo[i]=(get_pos - position_servo_mul[i])0.084coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
Artiom	5:3de53c9af201	216	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Artiom	5:3de53c9af201	217	flag_correction = 1;
Artiom	5:3de53c9af201	218	} else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
Artiom	5:3de53c9af201	219	tab_tempo[i]=(position_servo_mul[i] - get_pos)0.084coeffient_time+1;
Artiom	5:3de53c9af201	220	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Artiom	5:3de53c9af201	221	flag_correction = 1;
Artiom	5:3de53c9af201	222	}
Artiom	5:3de53c9af201	223	}
Artiom	5:3de53c9af201	224	if(flag_correction == 1) {
Artiom	5:3de53c9af201	225	new_tempo = 0;
Artiom	5:3de53c9af201	226	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	227	if(tab_tempo[i]>new_tempo) {
Artiom	5:3de53c9af201	228	new_tempo = tab_tempo[i];
Artiom	5:3de53c9af201	229	}
Artiom	5:3de53c9af201	230	}
Artiom	5:3de53c9af201	231	flag_correction = 0;
Artiom	5:3de53c9af201	232	positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul,serial_numero);
kyxstark	6:81733a7b69e9	233	//pc.printf("Correction!\n");
Artiom	5:3de53c9af201	234	}
Artiom	5:3de53c9af201	235	veri = 0;
Artiom	5:3de53c9af201	236	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	237	get_pos = getPos(data_servo_mul[1+2*i],serial_numero);
kyxstark	6:81733a7b69e9	238	//pc.printf("PosiM=%d ",get_pos);
Artiom	5:3de53c9af201	239	if((get_pos - position_servo_mul[i])>tolerance_en_position) {
Artiom	5:3de53c9af201	240	tab_tempo[i]=(get_pos - position_servo_mul[i])0.084coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
Artiom	5:3de53c9af201	241	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Artiom	5:3de53c9af201	242	flag_correction = 1;
Artiom	5:3de53c9af201	243	} else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
Artiom	5:3de53c9af201	244	tab_tempo[i]=(position_servo_mul[i] - get_pos)0.084coeffient_time+1;
Artiom	5:3de53c9af201	245	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Artiom	5:3de53c9af201	246	flag_correction = 1;
Artiom	5:3de53c9af201	247	} else { //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif))
Artiom	5:3de53c9af201	248	veri++;
Artiom	5:3de53c9af201	249	}
Artiom	5:3de53c9af201	250	}
Artiom	5:3de53c9af201	251	}
Artiom	5:3de53c9af201	252	break;
Artiom	5:3de53c9af201	253	case pos_mul_complex_different:
Artiom	5:3de53c9af201	254	//---------------------------Status---------------------------
Artiom	5:3de53c9af201	255	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	256	Status = getStatus(data_servo_mul_different[1+3*i],serial_numero);
kyxstark	6:81733a7b69e9	257	////pc.printf("status = %d",Status);
Artiom	5:3de53c9af201	258	switch(Status) {
Artiom	5:3de53c9af201	259	case 0:
Artiom	5:3de53c9af201	260	break;
Artiom	5:3de53c9af201	261
Artiom	5:3de53c9af201	262	case 2: //Exceed allowed POT limit
kyxstark	6:81733a7b69e9	263	////pc.printf("ERR-Depasse la limite de position\n");
Artiom	5:3de53c9af201	264	//clean_ERR(id);
Artiom	5:3de53c9af201	265	//wait_ms(500);
Artiom	5:3de53c9af201	266	clear(data_servo_mul_different[1+3*i],serial_numero);
Artiom	5:3de53c9af201	267	//positionControl(id, 1000, 3, GLED_ON);
Artiom	5:3de53c9af201	268	//wait_ms(3);
Artiom	5:3de53c9af201	269	//Status = getStatus(data_servo_mul_different[1+2*i]);
Artiom	5:3de53c9af201	270	//wait_ms(3);
kyxstark	6:81733a7b69e9	271	////pc.printf("status = %d",Status);
Artiom	5:3de53c9af201	272	break;
Artiom	5:3de53c9af201	273	}
Artiom	5:3de53c9af201	274	}
Artiom	5:3de53c9af201	275	//-------------------Torque et position-----------------------------
Artiom	5:3de53c9af201	276	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	277	my_Tor = Get_Torque(data_servo_mul_different[1+3*i],serial_numero);
Artiom	5:3de53c9af201	278	while(my_Tor != 0x60) {
Artiom	5:3de53c9af201	279	setTorque(data_servo_mul_different[1+3*i],TORQUE_ON,serial_numero);
Artiom	5:3de53c9af201	280	my_Tor = Get_Torque(data_servo_mul_different[1+3*i],serial_numero);
Artiom	5:3de53c9af201	281	//wait_ms(5);
kyxstark	6:81733a7b69e9	282	////pc.printf(" SET_TORQUE ");
Artiom	5:3de53c9af201	283	}
Artiom	5:3de53c9af201	284	}
Artiom	5:3de53c9af201	285	for(i=0; i<nombre_servo; i++) {
Artiom	5:3de53c9af201	286	Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i],serial_numero);
Artiom	5:3de53c9af201	287	Tension = Tension_inter*0.074;
Artiom	5:3de53c9af201	288	if(Tension <=6.60) {
Artiom	5:3de53c9af201	289	coeffient_time = 6;
Artiom	5:3de53c9af201	290	} else if(Tension <= 6.90) {
Artiom	5:3de53c9af201	291	coeffient_time = 4;
Artiom	5:3de53c9af201	292	} else if(Tension <= 7.10) {
Artiom	5:3de53c9af201	293	coeffient_time = 2;
Artiom	5:3de53c9af201	294	} else if(Tension > 7.10) {
Artiom	5:3de53c9af201	295	coeffient_time = 1;
Artiom	5:3de53c9af201	296	}
Artiom	5:3de53c9af201	297	get_pos = getPos(data_servo_mul_different[1+3*i],serial_numero);
kyxstark	6:81733a7b69e9	298	//pc.printf("PosiM=%d ",get_pos);
Artiom	5:3de53c9af201	299	if((get_pos - position_servo_mul_different[i])>tolerance_en_position) {
Artiom	5:3de53c9af201	300	tab_tempo[i]=(get_pos - position_servo_mul_different[i])0.084coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
Artiom	5:3de53c9af201	301	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Artiom	5:3de53c9af201	302	data_servo_mul_different[2+3*i] = tab_tempo[i];
Artiom	5:3de53c9af201	303	flag_correction = 1;
Artiom	5:3de53c9af201	304	} else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) {
Artiom	5:3de53c9af201	305	tab_tempo[i]=(position_servo_mul_different[i] - get_pos)0.084coeffient_time+1;
Artiom	5:3de53c9af201	306	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Artiom	5:3de53c9af201	307	data_servo_mul_different[2+3*i] = tab_tempo[i];
Artiom	5:3de53c9af201	308	flag_correction = 1;
Artiom	5:3de53c9af201	309	}
Artiom	5:3de53c9af201	310	}
Artiom	5:3de53c9af201	311	if(flag_correction == 1) {
Artiom	5:3de53c9af201	312	flag_correction = 0;
Artiom	5:3de53c9af201	313	positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different,serial_numero);
kyxstark	6:81733a7b69e9	314	//pc.printf("Correction!\n");
Artiom	5:3de53c9af201	315	}
Artiom	5:3de53c9af201	316	break;
Artiom	5:3de53c9af201	317	}
Artiom	11:542de77801ba	318	return true;
Artiom	5:3de53c9af201	319	}
AdrienSalle	0:32fd5a08c430	320	//
AdrienSalle	0:32fd5a08c430	321	//La fonction automate_serial1() sert à vérifier la bonne réception des données
AdrienSalle	0:32fd5a08c430	322	//elle est automatiquement appelée par la fonction receive_serial1()
AdrienSalle	0:32fd5a08c430	323	//
AdrienSalle	0:32fd5a08c430	324	void automate_serial1()
AdrienSalle	0:32fd5a08c430	325	{
AdrienSalle	0:32fd5a08c430	326	typedef enum {Attente,FF,Size,Data} type_etat1;
AdrienSalle	0:32fd5a08c430	327	static type_etat1 etat1=Attente;
kyxstark	6:81733a7b69e9	328	///////pc.printf("coucou");
AdrienSalle	0:32fd5a08c430	329
kyxstark	6:81733a7b69e9	330	//////pc.printf("%d\r\n", char_receive_serial1[valueserial1]);
AdrienSalle	0:32fd5a08c430	331
AdrienSalle	0:32fd5a08c430	332	switch (etat1) {
AdrienSalle	0:32fd5a08c430	333	// état Attente
AdrienSalle	0:32fd5a08c430	334	//on attend la réception des données
AdrienSalle	0:32fd5a08c430	335	//si on reçois l'octet 0xFF, il s'agit d'un début de trame
AdrienSalle	0:32fd5a08c430	336	//on passe à l'état suivant
AdrienSalle	0:32fd5a08c430	337	//
AdrienSalle	0:32fd5a08c430	338	case Attente:
AdrienSalle	0:32fd5a08c430	339	if(char_receive_serial1[0] == 0xFF) {
AdrienSalle	0:32fd5a08c430	340	etat1 = FF;
AdrienSalle	0:32fd5a08c430	341	valueserial1 = 1;
AdrienSalle	0:32fd5a08c430	342	}
AdrienSalle	0:32fd5a08c430	343	break;
AdrienSalle	0:32fd5a08c430	344	// état FF
AdrienSalle	0:32fd5a08c430	345	//on attend le second octet 0xFF pour confirmer qu'il s'agit d'une trame
AdrienSalle	0:32fd5a08c430	346	//si on reçoit l'octet 0xFF, il s'agit bien d'une trame Herkulex
AdrienSalle	0:32fd5a08c430	347	//on passe à l'état suivant
AdrienSalle	0:32fd5a08c430	348	//Sinon on retourne à l'état précédent
AdrienSalle	0:32fd5a08c430	349	//
AdrienSalle	0:32fd5a08c430	350	case FF:
AdrienSalle	0:32fd5a08c430	351	if(char_receive_serial1[1] == 0xFF) {
AdrienSalle	0:32fd5a08c430	352	etat1 = Size;
AdrienSalle	0:32fd5a08c430	353	valueserial1 = 2;
AdrienSalle	0:32fd5a08c430	354	} else {
AdrienSalle	0:32fd5a08c430	355	etat1 = Attente;
AdrienSalle	0:32fd5a08c430	356	valueserial1 = 0;
AdrienSalle	0:32fd5a08c430	357	flag_perdu_info_serial1 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	358	}
AdrienSalle	0:32fd5a08c430	359	break;
AdrienSalle	0:32fd5a08c430	360	// état size
AdrienSalle	0:32fd5a08c430	361	//On vérifie si l'octet size est supérieur à la taille minimale d'une trame Herkulex,
AdrienSalle	0:32fd5a08c430	362	//Si oui on passe à l'état suivant
AdrienSalle	0:32fd5a08c430	363	//Sinon on passe à l'état attente et flag_perdu_info_serial1 passe à 1 pour signaler la perte d'information
AdrienSalle	0:32fd5a08c430	364	case Size:
AdrienSalle	0:32fd5a08c430	365	if(char_receive_serial1[2] < 7) {
AdrienSalle	0:32fd5a08c430	366	etat1 = Attente;
AdrienSalle	0:32fd5a08c430	367	valueserial1 = 0;
AdrienSalle	0:32fd5a08c430	368	flag_perdu_info_serial1 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	369	} else {
AdrienSalle	0:32fd5a08c430	370	etat1 = Data;
AdrienSalle	0:32fd5a08c430	371	old_valueserial1 = 2;
AdrienSalle	0:32fd5a08c430	372	valueserial1 = 3;
AdrienSalle	0:32fd5a08c430	373	}
AdrienSalle	0:32fd5a08c430	374	Size_trame_serial1 = char_receive_serial1[2];
AdrienSalle	0:32fd5a08c430	375	break;
AdrienSalle	0:32fd5a08c430	376	//état data
AdrienSalle	0:32fd5a08c430	377	//on verifie que la taille de la trame reçue correspond à celle indiquée dans l'octet 'size'
AdrienSalle	0:32fd5a08c430	378	//si oui
AdrienSalle	0:32fd5a08c430	379	//flag_serial1_receive passe à 1 pour indiquer que la trame à bien été transmise
AdrienSalle	0:32fd5a08c430	380	case Data:
AdrienSalle	0:32fd5a08c430	381	if((valueserial1-2)==(Size_trame_serial1-3)) {
AdrienSalle	0:32fd5a08c430	382	flag_serial1_receive = 1;
AdrienSalle	0:32fd5a08c430	383	etat1 = Attente;
AdrienSalle	0:32fd5a08c430	384	valueserial1 = 0;
AdrienSalle	0:32fd5a08c430	385	} else {
AdrienSalle	0:32fd5a08c430	386	valueserial1++;
AdrienSalle	0:32fd5a08c430	387	}
AdrienSalle	0:32fd5a08c430	388	break;
AdrienSalle	0:32fd5a08c430	389
AdrienSalle	0:32fd5a08c430	390	default:
AdrienSalle	0:32fd5a08c430	391	break;
AdrienSalle	0:32fd5a08c430	392	}
AdrienSalle	0:32fd5a08c430	393	}
AdrienSalle	0:32fd5a08c430	394	//---------------------fonction d'interruption de reception de serial2---------------------
AdrienSalle	0:32fd5a08c430	395	//même principe que la fonction d'interrutpion de serial1
AdrienSalle	0:32fd5a08c430	396	unsigned char flag_perdu_info_serial2 = 0, indicateur_serial2 = 0, Size_trame_serial2 = 0, old_valueserial2 = 0;
AdrienSalle	0:32fd5a08c430	397	unsigned char char_receive_serial2[100];
AdrienSalle	0:32fd5a08c430	398	unsigned char valueserial2=0;
AdrienSalle	0:32fd5a08c430	399	unsigned char flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	400	void receive_serial2()
AdrienSalle	0:32fd5a08c430	401	{
AdrienSalle	0:32fd5a08c430	402	char_receive_serial2[valueserial2]=serial2.getc();
AdrienSalle	0:32fd5a08c430	403	automate_serial2();
AdrienSalle	0:32fd5a08c430	404	}
AdrienSalle	0:32fd5a08c430	405
AdrienSalle	0:32fd5a08c430	406	void Interrupt2_en(void)
AdrienSalle	0:32fd5a08c430	407	{
AdrienSalle	0:32fd5a08c430	408	serial2.attach(&receive_serial2,Serial::RxIrq);
AdrienSalle	0:32fd5a08c430	409	}
AdrienSalle	0:32fd5a08c430	410
AdrienSalle	0:32fd5a08c430	411	void automate_serial2()
AdrienSalle	0:32fd5a08c430	412	{
AdrienSalle	0:32fd5a08c430	413	typedef enum {Attente,FF,Size,Data} type_etat2;
AdrienSalle	0:32fd5a08c430	414	static type_etat2 etat2=Attente;
kyxstark	6:81733a7b69e9	415	//////////pc.printf("coucou");
AdrienSalle	0:32fd5a08c430	416
kyxstark	6:81733a7b69e9	417	//////pc.printf("%d\r\n", char_receive_serial2[valueserial2]);
AdrienSalle	0:32fd5a08c430	418
AdrienSalle	0:32fd5a08c430	419	switch (etat2) {
AdrienSalle	0:32fd5a08c430	420	case Attente:
AdrienSalle	0:32fd5a08c430	421	if(char_receive_serial2[0] == 0xFF) {
AdrienSalle	0:32fd5a08c430	422	etat2 = FF;
AdrienSalle	0:32fd5a08c430	423	valueserial2 = 1;
AdrienSalle	0:32fd5a08c430	424	}
AdrienSalle	0:32fd5a08c430	425	break;
AdrienSalle	0:32fd5a08c430	426	case FF:
AdrienSalle	0:32fd5a08c430	427	if(char_receive_serial2[1] == 0xFF) {
AdrienSalle	0:32fd5a08c430	428	etat2 = Size;
AdrienSalle	0:32fd5a08c430	429	valueserial2 = 2;
AdrienSalle	0:32fd5a08c430	430	} else {
AdrienSalle	0:32fd5a08c430	431	etat2 = Attente;
AdrienSalle	0:32fd5a08c430	432	valueserial2 = 0;
AdrienSalle	0:32fd5a08c430	433	flag_perdu_info_serial2 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	434	}
AdrienSalle	0:32fd5a08c430	435	break;
AdrienSalle	0:32fd5a08c430	436	case Size:
AdrienSalle	0:32fd5a08c430	437	if(char_receive_serial2[2] < 7) {
AdrienSalle	0:32fd5a08c430	438	etat2 = Attente;
AdrienSalle	0:32fd5a08c430	439	valueserial2 = 0;
AdrienSalle	0:32fd5a08c430	440	flag_perdu_info_serial2 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	441	} else {
AdrienSalle	0:32fd5a08c430	442	etat2 = Data;
AdrienSalle	0:32fd5a08c430	443	old_valueserial2 = 2;
AdrienSalle	0:32fd5a08c430	444	valueserial2 = 3;
AdrienSalle	0:32fd5a08c430	445	}
AdrienSalle	0:32fd5a08c430	446	Size_trame_serial2 = char_receive_serial2[2];
AdrienSalle	0:32fd5a08c430	447	break;
AdrienSalle	0:32fd5a08c430	448
AdrienSalle	0:32fd5a08c430	449	case Data:
AdrienSalle	0:32fd5a08c430	450	if((valueserial2-2)==(Size_trame_serial2-3)) {
AdrienSalle	0:32fd5a08c430	451	flag_serial2_receive = 1;
AdrienSalle	0:32fd5a08c430	452	etat2 = Attente;
AdrienSalle	0:32fd5a08c430	453	valueserial2 = 0;
AdrienSalle	0:32fd5a08c430	454	} else {
AdrienSalle	0:32fd5a08c430	455	valueserial2++;
AdrienSalle	0:32fd5a08c430	456	}
AdrienSalle	0:32fd5a08c430	457	break;
AdrienSalle	0:32fd5a08c430	458
AdrienSalle	0:32fd5a08c430	459	default:
AdrienSalle	0:32fd5a08c430	460	break;
AdrienSalle	0:32fd5a08c430	461	}
AdrienSalle	0:32fd5a08c430	462	}
AdrienSalle	0:32fd5a08c430	463	//---------------------fonction d'interruption de reception de serial3---------------------
AdrienSalle	0:32fd5a08c430	464	//même principe que la fonction d'interrutpion de serial1
AdrienSalle	0:32fd5a08c430	465	unsigned char flag_perdu_info_serial3 = 0, indicateur_serial3 = 0, Size_trame_serial3 = 0, old_valueserial3 = 0;
AdrienSalle	0:32fd5a08c430	466	unsigned char char_receive_serial3[100];
AdrienSalle	0:32fd5a08c430	467	unsigned char valueserial3=0;
AdrienSalle	0:32fd5a08c430	468	unsigned char flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	469	void receive_serial3()
AdrienSalle	0:32fd5a08c430	470	{
AdrienSalle	0:32fd5a08c430	471	char_receive_serial3[valueserial3]=serial3.getc();
AdrienSalle	0:32fd5a08c430	472	automate_serial3();
AdrienSalle	0:32fd5a08c430	473	}
AdrienSalle	0:32fd5a08c430	474
AdrienSalle	0:32fd5a08c430	475	void Interrupt3_en(void)
AdrienSalle	0:32fd5a08c430	476	{
AdrienSalle	0:32fd5a08c430	477	serial3.attach(&receive_serial3,Serial::RxIrq);
AdrienSalle	0:32fd5a08c430	478	}
AdrienSalle	0:32fd5a08c430	479
AdrienSalle	0:32fd5a08c430	480	void automate_serial3()
AdrienSalle	0:32fd5a08c430	481	{
AdrienSalle	0:32fd5a08c430	482	typedef enum {Attente,FF,Size,Data} type_etat3;
AdrienSalle	0:32fd5a08c430	483	static type_etat3 etat3=Attente;
kyxstark	6:81733a7b69e9	484	//////////pc.printf("coucou");
AdrienSalle	0:32fd5a08c430	485
kyxstark	6:81733a7b69e9	486	//////pc.printf("%d\r\n", char_receive_serial3[valueserial3]);
AdrienSalle	0:32fd5a08c430	487
AdrienSalle	0:32fd5a08c430	488	switch (etat3) {
AdrienSalle	0:32fd5a08c430	489	case Attente:
AdrienSalle	0:32fd5a08c430	490	if(char_receive_serial3[0] == 0xFF) {
AdrienSalle	0:32fd5a08c430	491	etat3 = FF;
AdrienSalle	0:32fd5a08c430	492	valueserial3 = 1;
AdrienSalle	0:32fd5a08c430	493	}
AdrienSalle	0:32fd5a08c430	494	break;
AdrienSalle	0:32fd5a08c430	495	case FF:
AdrienSalle	0:32fd5a08c430	496	if(char_receive_serial3[1] == 0xFF) {
AdrienSalle	0:32fd5a08c430	497	etat3 = Size;
AdrienSalle	0:32fd5a08c430	498	valueserial3 = 2;
AdrienSalle	0:32fd5a08c430	499	} else {
AdrienSalle	0:32fd5a08c430	500	etat3 = Attente;
AdrienSalle	0:32fd5a08c430	501	valueserial3 = 0;
AdrienSalle	0:32fd5a08c430	502	flag_perdu_info_serial3 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	503	}
AdrienSalle	0:32fd5a08c430	504	break;
AdrienSalle	0:32fd5a08c430	505	case Size:
AdrienSalle	0:32fd5a08c430	506	if(char_receive_serial3[2] < 7) {
AdrienSalle	0:32fd5a08c430	507	etat3 = Attente;
AdrienSalle	0:32fd5a08c430	508	valueserial3 = 0;
AdrienSalle	0:32fd5a08c430	509	flag_perdu_info_serial3 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	510	} else {
AdrienSalle	0:32fd5a08c430	511	etat3 = Data;
AdrienSalle	0:32fd5a08c430	512	old_valueserial3 = 2;
AdrienSalle	0:32fd5a08c430	513	valueserial3 = 3;
AdrienSalle	0:32fd5a08c430	514	}
AdrienSalle	0:32fd5a08c430	515	Size_trame_serial3 = char_receive_serial3[2];
AdrienSalle	0:32fd5a08c430	516	break;
AdrienSalle	0:32fd5a08c430	517
AdrienSalle	0:32fd5a08c430	518	case Data:
AdrienSalle	0:32fd5a08c430	519	if((valueserial3-2)==(Size_trame_serial3-3)) {
AdrienSalle	0:32fd5a08c430	520	flag_serial3_receive = 1;
AdrienSalle	0:32fd5a08c430	521	etat3 = Attente;
AdrienSalle	0:32fd5a08c430	522	valueserial3 = 0;
AdrienSalle	0:32fd5a08c430	523	} else {
AdrienSalle	0:32fd5a08c430	524	valueserial3++;
AdrienSalle	0:32fd5a08c430	525	}
AdrienSalle	0:32fd5a08c430	526	break;
AdrienSalle	0:32fd5a08c430	527
AdrienSalle	0:32fd5a08c430	528	default:
AdrienSalle	0:32fd5a08c430	529	break;
AdrienSalle	0:32fd5a08c430	530	}
AdrienSalle	0:32fd5a08c430	531	}
AdrienSalle	0:32fd5a08c430	532	//---------------------fonction d'interruption de reception de serial4---------------------
AdrienSalle	0:32fd5a08c430	533	//même principe que la fonction d'interrutpion de serial1
AdrienSalle	0:32fd5a08c430	534	unsigned char flag_perdu_info_serial4 = 0, indicateur_serial4 = 0, Size_trame_serial4 = 0, old_valueserial4 = 0;
AdrienSalle	0:32fd5a08c430	535	unsigned char char_receive_serial4[100];
AdrienSalle	0:32fd5a08c430	536	unsigned char valueserial4=0;
AdrienSalle	0:32fd5a08c430	537	unsigned char flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	538	void receive_serial4()
AdrienSalle	0:32fd5a08c430	539	{
AdrienSalle	0:32fd5a08c430	540	char_receive_serial4[valueserial4]=serial4.getc();
AdrienSalle	0:32fd5a08c430	541	automate_serial4();
AdrienSalle	0:32fd5a08c430	542	}
AdrienSalle	0:32fd5a08c430	543
AdrienSalle	0:32fd5a08c430	544	void Interrupt4_en(void)
AdrienSalle	0:32fd5a08c430	545	{
AdrienSalle	0:32fd5a08c430	546	serial4.attach(&receive_serial4,Serial::RxIrq);
AdrienSalle	0:32fd5a08c430	547	}
AdrienSalle	0:32fd5a08c430	548
AdrienSalle	0:32fd5a08c430	549	void automate_serial4()
AdrienSalle	0:32fd5a08c430	550	{
AdrienSalle	0:32fd5a08c430	551	typedef enum {Attente,FF,Size,Data} type_etat4;
AdrienSalle	0:32fd5a08c430	552	static type_etat4 etat4=Attente;
kyxstark	6:81733a7b69e9	553	//////////pc.printf("coucou");
AdrienSalle	0:32fd5a08c430	554
kyxstark	6:81733a7b69e9	555	//////pc.printf("%d\r\n", char_receive_serial4[valueserial4]);
AdrienSalle	0:32fd5a08c430	556
AdrienSalle	0:32fd5a08c430	557	switch (etat4) {
AdrienSalle	0:32fd5a08c430	558	case Attente:
AdrienSalle	0:32fd5a08c430	559	if(char_receive_serial4[0] == 0xFF) {
AdrienSalle	0:32fd5a08c430	560	etat4 = FF;
AdrienSalle	0:32fd5a08c430	561	valueserial4 = 1;
AdrienSalle	0:32fd5a08c430	562	}
AdrienSalle	0:32fd5a08c430	563	break;
AdrienSalle	0:32fd5a08c430	564	case FF:
AdrienSalle	0:32fd5a08c430	565	if(char_receive_serial4[1] == 0xFF) {
AdrienSalle	0:32fd5a08c430	566	etat4 = Size;
AdrienSalle	0:32fd5a08c430	567	valueserial4 = 2;
AdrienSalle	0:32fd5a08c430	568	} else {
AdrienSalle	0:32fd5a08c430	569	etat4 = Attente;
AdrienSalle	0:32fd5a08c430	570	valueserial4 = 0;
AdrienSalle	0:32fd5a08c430	571	flag_perdu_info_serial4 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	572	}
AdrienSalle	0:32fd5a08c430	573	break;
AdrienSalle	0:32fd5a08c430	574	case Size:
AdrienSalle	0:32fd5a08c430	575	if(char_receive_serial4[2] < 7) {
AdrienSalle	0:32fd5a08c430	576	etat4 = Attente;
AdrienSalle	0:32fd5a08c430	577	valueserial4 = 0;
AdrienSalle	0:32fd5a08c430	578	flag_perdu_info_serial4 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	579	} else {
AdrienSalle	0:32fd5a08c430	580	etat4 = Data;
AdrienSalle	0:32fd5a08c430	581	old_valueserial4 = 2;
AdrienSalle	0:32fd5a08c430	582	valueserial4 = 3;
AdrienSalle	0:32fd5a08c430	583	}
AdrienSalle	0:32fd5a08c430	584	Size_trame_serial4 = char_receive_serial4[2];
AdrienSalle	0:32fd5a08c430	585	break;
AdrienSalle	0:32fd5a08c430	586
AdrienSalle	0:32fd5a08c430	587	case Data:
AdrienSalle	0:32fd5a08c430	588	if((valueserial4-2)==(Size_trame_serial4-3)) {
AdrienSalle	0:32fd5a08c430	589	flag_serial4_receive = 1;
AdrienSalle	0:32fd5a08c430	590	etat4 = Attente;
AdrienSalle	0:32fd5a08c430	591	valueserial4 = 0;
AdrienSalle	0:32fd5a08c430	592	} else {
AdrienSalle	0:32fd5a08c430	593	valueserial4++;
AdrienSalle	0:32fd5a08c430	594	}
AdrienSalle	0:32fd5a08c430	595	break;
AdrienSalle	0:32fd5a08c430	596
AdrienSalle	0:32fd5a08c430	597	default:
AdrienSalle	0:32fd5a08c430	598	break;
AdrienSalle	0:32fd5a08c430	599	}
AdrienSalle	0:32fd5a08c430	600	}
AdrienSalle	0:32fd5a08c430	601	//---------------------fonction d'interruption de reception de serial5---------------------
AdrienSalle	0:32fd5a08c430	602	//même principe que la fonction d'interrutpion de serial1
AdrienSalle	0:32fd5a08c430	603	unsigned char flag_perdu_info_serial5 = 0, indicateur_serial5 = 0, Size_trame_serial5 = 0, old_valueserial5 = 0;
AdrienSalle	0:32fd5a08c430	604	unsigned char char_receive_serial5[100];
AdrienSalle	0:32fd5a08c430	605	unsigned char valueserial5=0;
AdrienSalle	0:32fd5a08c430	606	unsigned char flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	607	void receive_serial5()
AdrienSalle	0:32fd5a08c430	608	{
AdrienSalle	0:32fd5a08c430	609	char_receive_serial5[valueserial5]=serial5.getc();
AdrienSalle	0:32fd5a08c430	610	automate_serial5();
AdrienSalle	0:32fd5a08c430	611	}
AdrienSalle	0:32fd5a08c430	612
AdrienSalle	0:32fd5a08c430	613	void Interrupt5_en(void)
AdrienSalle	0:32fd5a08c430	614	{
AdrienSalle	0:32fd5a08c430	615	serial5.attach(&receive_serial5,Serial::RxIrq);
AdrienSalle	0:32fd5a08c430	616	}
AdrienSalle	0:32fd5a08c430	617
AdrienSalle	0:32fd5a08c430	618	void automate_serial5()
AdrienSalle	0:32fd5a08c430	619	{
AdrienSalle	0:32fd5a08c430	620	typedef enum {Attente,FF,Size,Data} type_etat5;
AdrienSalle	0:32fd5a08c430	621	static type_etat5 etat5=Attente;
kyxstark	6:81733a7b69e9	622	//////////pc.printf("coucou");
AdrienSalle	0:32fd5a08c430	623
kyxstark	6:81733a7b69e9	624	//////pc.printf("%d\r\n", char_receive_serial5[valueserial5]);
AdrienSalle	0:32fd5a08c430	625
AdrienSalle	0:32fd5a08c430	626	switch (etat5) {
AdrienSalle	0:32fd5a08c430	627	case Attente:
AdrienSalle	0:32fd5a08c430	628	if(char_receive_serial5[0] == 0xFF) {
AdrienSalle	0:32fd5a08c430	629	etat5 = FF;
AdrienSalle	0:32fd5a08c430	630	valueserial5 = 1;
AdrienSalle	0:32fd5a08c430	631	}
AdrienSalle	0:32fd5a08c430	632	break;
AdrienSalle	0:32fd5a08c430	633	case FF:
AdrienSalle	0:32fd5a08c430	634	if(char_receive_serial5[1] == 0xFF) {
AdrienSalle	0:32fd5a08c430	635	etat5 = Size;
AdrienSalle	0:32fd5a08c430	636	valueserial5 = 2;
AdrienSalle	0:32fd5a08c430	637	} else {
AdrienSalle	0:32fd5a08c430	638	etat5 = Attente;
AdrienSalle	0:32fd5a08c430	639	valueserial5 = 0;
AdrienSalle	0:32fd5a08c430	640	flag_perdu_info_serial5 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	641	}
AdrienSalle	0:32fd5a08c430	642	break;
AdrienSalle	0:32fd5a08c430	643	case Size:
AdrienSalle	0:32fd5a08c430	644	if(char_receive_serial5[2] < 7) {
AdrienSalle	0:32fd5a08c430	645	etat5 = Attente;
AdrienSalle	0:32fd5a08c430	646	valueserial5 = 0;
AdrienSalle	0:32fd5a08c430	647	flag_perdu_info_serial5 = 1; //flag_perdu_info_serial1
AdrienSalle	0:32fd5a08c430	648	} else {
AdrienSalle	0:32fd5a08c430	649	etat5 = Data;
AdrienSalle	0:32fd5a08c430	650	old_valueserial5 = 2;
AdrienSalle	0:32fd5a08c430	651	valueserial5 = 3;
AdrienSalle	0:32fd5a08c430	652	}
AdrienSalle	0:32fd5a08c430	653	Size_trame_serial5 = char_receive_serial5[2];
AdrienSalle	0:32fd5a08c430	654	break;
AdrienSalle	0:32fd5a08c430	655
AdrienSalle	0:32fd5a08c430	656	case Data:
AdrienSalle	0:32fd5a08c430	657	if((valueserial5-2)==(Size_trame_serial5-3)) {
AdrienSalle	0:32fd5a08c430	658	flag_serial5_receive = 1;
AdrienSalle	0:32fd5a08c430	659	etat5 = Attente;
AdrienSalle	0:32fd5a08c430	660	valueserial5 = 0;
AdrienSalle	0:32fd5a08c430	661	} else {
AdrienSalle	0:32fd5a08c430	662	valueserial5++;
AdrienSalle	0:32fd5a08c430	663	}
AdrienSalle	0:32fd5a08c430	664	break;
AdrienSalle	0:32fd5a08c430	665
AdrienSalle	0:32fd5a08c430	666	default:
AdrienSalle	0:32fd5a08c430	667	break;
AdrienSalle	0:32fd5a08c430	668	}
AdrienSalle	0:32fd5a08c430	669	}
AdrienSalle	0:32fd5a08c430	670	//----------------xxxxx----fonction de fermture de serial-----------------------
AdrienSalle	0:32fd5a08c430	671	/*void N_Herkulex()
AdrienSalle	0:32fd5a08c430	672	{
AdrienSalle	0:32fd5a08c430	673
AdrienSalle	0:32fd5a08c430	674	if(Sv != NULL)
AdrienSalle	0:32fd5a08c430	675	delete Sv;
AdrienSalle	0:32fd5a08c430	676	if(recevoir==2) {
AdrienSalle	0:32fd5a08c430	677	size_reponse = rx2[recevoir];
AdrienSalle	0:32fd5a08c430	678	}
AdrienSalle	0:32fd5a08c430	679	}*/
AdrienSalle	0:32fd5a08c430	680	//-------------------------fonction de transmission-----------------------------
AdrienSalle	0:32fd5a08c430	681	//
AdrienSalle	0:32fd5a08c430	682	//Permet de transmettre une trame manuellement sur une liaison série choisie
AdrienSalle	0:32fd5a08c430	683	//
AdrienSalle	0:32fd5a08c430	684	//packetSize ==> Taille totale de la trame en octets
AdrienSalle	0:32fd5a08c430	685	// en-têtes (HEADER) et données (data) inclus
AdrienSalle	0:32fd5a08c430	686	//
AdrienSalle	0:32fd5a08c430	687	//data ==> Données ( Ici il d'agit de la trame en entier) à rentrer sous forme de tableau (1 octet par case!)
AdrienSalle	0:32fd5a08c430	688	//
AdrienSalle	0:32fd5a08c430	689	//numero_serial ==> Numéro de la liaison série sur laquelle on
AdrienSalle	0:32fd5a08c430	690	// envoie la trame
AdrienSalle	0:32fd5a08c430	691	void txPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	692
AdrienSalle	0:32fd5a08c430	693
AdrienSalle	0:32fd5a08c430	694	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	695	pc->printf("[TX]");
AdrienSalle	0:32fd5a08c430	696	for(uint8_t i = 0; i < packetSize ; i++)
AdrienSalle	0:32fd5a08c430	697	{
AdrienSalle	0:32fd5a08c430	698	#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	699	pc->printf("%02X ",data[i]);
AdrienSalle	0:32fd5a08c430	700	#endif
AdrienSalle	0:32fd5a08c430	701	txd->putc(data[i]);
AdrienSalle	0:32fd5a08c430	702	}
AdrienSalle	0:32fd5a08c430	703	#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	704	pc->printf("\n");
AdrienSalle	0:32fd5a08c430	705	#endif*/
AdrienSalle	0:32fd5a08c430	706	{
AdrienSalle	0:32fd5a08c430	707	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	708	if(numero_serial == 1) { //Envoi sur la liaison série 1
AdrienSalle	0:32fd5a08c430	709	for(uint8_t i = 0; i < packetSize ; i++) { //
AdrienSalle	0:32fd5a08c430	710	while(serial1.writeable() == 0); //On envoie 1 octet toute les 100 us
AdrienSalle	0:32fd5a08c430	711	serial1.putc(data[i]); //
AdrienSalle	0:32fd5a08c430	712	wait_us(100); //
AdrienSalle	0:32fd5a08c430	713	}
AdrienSalle	0:32fd5a08c430	714	} else if(numero_serial == 2) { //Envoi sur la liaison série 2
AdrienSalle	0:32fd5a08c430	715	for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle	0:32fd5a08c430	716	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	717	serial2.putc(data[i]);
AdrienSalle	0:32fd5a08c430	718	wait_us(100);
AdrienSalle	0:32fd5a08c430	719	}
AdrienSalle	0:32fd5a08c430	720	} else if(numero_serial == 3) { //Envoi sur la liaison série 3
AdrienSalle	0:32fd5a08c430	721	for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle	0:32fd5a08c430	722	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	723	serial3.putc(data[i]);
AdrienSalle	0:32fd5a08c430	724	wait_us(100);
AdrienSalle	0:32fd5a08c430	725	}
AdrienSalle	0:32fd5a08c430	726	} else if(numero_serial == 4) { //Envoi sur la liaison série 4
AdrienSalle	0:32fd5a08c430	727	for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle	0:32fd5a08c430	728	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	729	serial4.putc(data[i]);
AdrienSalle	0:32fd5a08c430	730	wait_us(100);
AdrienSalle	0:32fd5a08c430	731	}
AdrienSalle	0:32fd5a08c430	732	} else if(numero_serial == 5) { //Envoi sur la liaison série 5
AdrienSalle	0:32fd5a08c430	733	for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle	0:32fd5a08c430	734	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	735	serial5.putc(data[i]);
AdrienSalle	0:32fd5a08c430	736	wait_us(100);
AdrienSalle	0:32fd5a08c430	737	}
AdrienSalle	0:32fd5a08c430	738	}
AdrienSalle	0:32fd5a08c430	739	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	740	}
AdrienSalle	0:32fd5a08c430	741	//----------------------------fonction de reception-----------------------------
AdrienSalle	0:32fd5a08c430	742	//Permet de recevoir une trame
AdrienSalle	0:32fd5a08c430	743	void rxPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	744	//
AdrienSalle	0:32fd5a08c430	745	//packetSize ==> taille de la trame à recevoir
AdrienSalle	0:32fd5a08c430	746	//data ==> Données
AdrienSalle	0:32fd5a08c430	747	//
AdrienSalle	0:32fd5a08c430	748	{
AdrienSalle	0:32fd5a08c430	749
AdrienSalle	0:32fd5a08c430	750	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	751	pc->printf("[RX]");
AdrienSalle	0:32fd5a08c430	752	#endif
AdrienSalle	0:32fd5a08c430	753	for (uint8_t i=0; i < packetSize; i++)
AdrienSalle	0:32fd5a08c430	754	{
AdrienSalle	0:32fd5a08c430	755	data[i] = rxd->getc();
AdrienSalle	0:32fd5a08c430	756	#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	757	pc->printf("%02X ",data[i]);
AdrienSalle	0:32fd5a08c430	758	#endif
AdrienSalle	0:32fd5a08c430	759	}
AdrienSalle	0:32fd5a08c430	760	#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	761	pc->printf("\n");
AdrienSalle	0:32fd5a08c430	762	#endif*/
AdrienSalle	0:32fd5a08c430	763	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	764	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	765	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	766	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	767	data[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	768	//////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	769	}
AdrienSalle	0:32fd5a08c430	770	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	771	valueserial1=0;
AdrienSalle	0:32fd5a08c430	772	}
AdrienSalle	0:32fd5a08c430	773	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	774	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	775	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	776	data[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	777	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	778	}
AdrienSalle	0:32fd5a08c430	779	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	780	valueserial2=0;
AdrienSalle	0:32fd5a08c430	781	}
AdrienSalle	0:32fd5a08c430	782	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	783	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	784	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	785	data[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	786	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	787	}
AdrienSalle	0:32fd5a08c430	788	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	789	valueserial3=0;
AdrienSalle	0:32fd5a08c430	790	}
AdrienSalle	0:32fd5a08c430	791	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	792	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	793	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	794	data[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	795	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	796	}
AdrienSalle	0:32fd5a08c430	797	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	798	valueserial4=0;
AdrienSalle	0:32fd5a08c430	799	}
AdrienSalle	0:32fd5a08c430	800	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	801	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	802	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	803	data[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	804	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	805	}
AdrienSalle	0:32fd5a08c430	806	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	807	valueserial5=0;
AdrienSalle	0:32fd5a08c430	808	}
AdrienSalle	0:32fd5a08c430	809	}
AdrienSalle	0:32fd5a08c430	810	}
AdrienSalle	0:32fd5a08c430	811	//----------------------fonction pour sortir de l'état d'erreur-------------------------
AdrienSalle	0:32fd5a08c430	812	//
AdrienSalle	0:32fd5a08c430	813	//Permet de "sortir" de la mise en erreur d'un servomoteur
AdrienSalle	0:32fd5a08c430	814	//
AdrienSalle	0:32fd5a08c430	815	void clear(uint8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	816	//
AdrienSalle	0:32fd5a08c430	817	// id ==> On entre l'ID du servomoteur que l'on souhaite sortir de l'état d'erreur
AdrienSalle	0:32fd5a08c430	818	// numero serial ==> On entre le numéro de la liaison série sur laquelle se trouve le servomoteur concerné
AdrienSalle	0:32fd5a08c430	819	{
AdrienSalle	0:32fd5a08c430	820	uint8_t txBuf[11];
AdrienSalle	0:32fd5a08c430	821	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	822	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	823	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	824	txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
AdrienSalle	0:32fd5a08c430	825	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	826	txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) *On choisi le CMD pour écrire dans un registre
AdrienSalle	0:32fd5a08c430	827	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	828	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	829	txBuf[7] = RAM_STATUS_ERROR; // Address *On écrit dans le registre RAM_STATUS_ERROR
AdrienSalle	0:32fd5a08c430	830	txBuf[8] = BYTE2; // Length *
AdrienSalle	0:32fd5a08c430	831	txBuf[9] = 0; // Clear RAM_STATUS_ERROR
AdrienSalle	0:32fd5a08c430	832	txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
AdrienSalle	0:32fd5a08c430	833	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	834	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	835	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE; //calcul de checksum1
AdrienSalle	0:32fd5a08c430	836	txBuf[6] = (~txBuf[5])&0xFE; //calcul de checksum2
AdrienSalle	0:32fd5a08c430	837	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	838	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	839	for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle	0:32fd5a08c430	840	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	841	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	842	wait_us(100);
AdrienSalle	0:32fd5a08c430	843	}
AdrienSalle	0:32fd5a08c430	844	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	845	for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle	0:32fd5a08c430	846	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	847	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	848	wait_us(100);
AdrienSalle	0:32fd5a08c430	849	}
AdrienSalle	0:32fd5a08c430	850	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	851	for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle	0:32fd5a08c430	852	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	853	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	854	wait_us(100);
AdrienSalle	0:32fd5a08c430	855	}
AdrienSalle	0:32fd5a08c430	856	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	857	for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle	0:32fd5a08c430	858	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	859	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	860	wait_us(100);
AdrienSalle	0:32fd5a08c430	861	}
AdrienSalle	0:32fd5a08c430	862	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	863	for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle	0:32fd5a08c430	864	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	865	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	866	wait_us(100);
AdrienSalle	0:32fd5a08c430	867	}
AdrienSalle	0:32fd5a08c430	868	}
AdrienSalle	0:32fd5a08c430	869	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	870	}
AdrienSalle	0:32fd5a08c430	871	//----------------fonction de mis a jour le couple de servo---------------------
AdrienSalle	0:32fd5a08c430	872	void setTorque(uint8_t id, uint8_t cmdTorque, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	873	// Permet de modifier l'état du couple d'un servo------------
AdrienSalle	0:32fd5a08c430	874	// id ==> ID du servomoteur
AdrienSalle	0:32fd5a08c430	875	//cmdTorque ==> état souhaité pour le couple
AdrienSalle	0:32fd5a08c430	876	//numero_serial ==> Numéro de la liaison série sur laquelle se trouve le servo
AdrienSalle	0:32fd5a08c430	877
AdrienSalle	0:32fd5a08c430	878	// valeurs posssibles pour cmdTorque
AdrienSalle	0:32fd5a08c430	879	// 0x40 Break On Opérations commandes impossibles
AdrienSalle	0:32fd5a08c430	880	// 0x60 Torque On Fontionnement normal
AdrienSalle	0:32fd5a08c430	881	// 0x00 Torque Free Opérations commandes impossibles + possibilité de déplacer le servo manuellement
AdrienSalle	0:32fd5a08c430	882	{
AdrienSalle	0:32fd5a08c430	883	uint8_t txBuf[10];
AdrienSalle	0:32fd5a08c430	884	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	885	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	886	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	887	txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
AdrienSalle	0:32fd5a08c430	888	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	889	txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
AdrienSalle	0:32fd5a08c430	890	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	891	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	892	txBuf[7] = RAM_TORQUE_CONTROL; // Address
AdrienSalle	0:32fd5a08c430	893	txBuf[8] = BYTE1; // Length
AdrienSalle	0:32fd5a08c430	894	txBuf[9] = cmdTorque; // Torque ON
AdrienSalle	0:32fd5a08c430	895	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	896	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	897	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
AdrienSalle	0:32fd5a08c430	898	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	899	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	900	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	901	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	902	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	903	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	904	wait_us(100);
AdrienSalle	0:32fd5a08c430	905	}
AdrienSalle	0:32fd5a08c430	906	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	907	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	908	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	909	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	910	wait_us(100);
AdrienSalle	0:32fd5a08c430	911	}
AdrienSalle	0:32fd5a08c430	912	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	913	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	914	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	915	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	916	wait_us(100);
AdrienSalle	0:32fd5a08c430	917	}
AdrienSalle	0:32fd5a08c430	918	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	919	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	920	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	921	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	922	wait_us(100);
AdrienSalle	0:32fd5a08c430	923	}
AdrienSalle	0:32fd5a08c430	924	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	925	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	926	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	927	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	928	wait_us(100);
AdrienSalle	0:32fd5a08c430	929	}
AdrienSalle	0:32fd5a08c430	930	}
AdrienSalle	0:32fd5a08c430	931	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	932	}
AdrienSalle	0:32fd5a08c430	933	//-------------fonction de contrôle de position pour un seul servo--------------
AdrienSalle	0:32fd5a08c430	934	//Permet de controler un servomoteur en position
AdrienSalle	0:32fd5a08c430	935	void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	936	//
AdrienSalle	0:32fd5a08c430	937	//id ==> id du servo à déplacer
AdrienSalle	0:32fd5a08c430	938	//position ==> position à atteindre
AdrienSalle	0:32fd5a08c430	939	//playtime ==> temps à mettre pour effectuer le déplacement
AdrienSalle	0:32fd5a08c430	940	//setLED ==> LED à allumer
AdrienSalle	0:32fd5a08c430	941	//numero-serial ==> numéro de la liaison série
AdrienSalle	0:32fd5a08c430	942	{
AdrienSalle	0:32fd5a08c430	943	float tempo=0;
AdrienSalle	0:32fd5a08c430	944	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	945	//if (position > 1023) return; //1002-21
AdrienSalle	0:32fd5a08c430	946	if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
AdrienSalle	0:32fd5a08c430	947	tempo=playtime*0.012;
AdrienSalle	0:32fd5a08c430	948	pos_ID = id;
AdrienSalle	0:32fd5a08c430	949	uint8_t txBuf[12];
AdrienSalle	0:32fd5a08c430	950	etat = pos;
AdrienSalle	0:32fd5a08c430	951	pos_position = position;
AdrienSalle	0:32fd5a08c430	952	pos_time = playtime;
AdrienSalle	0:32fd5a08c430	953	pos_led = setLED;
AdrienSalle	0:32fd5a08c430	954	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	955	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	956	txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
AdrienSalle	0:32fd5a08c430	957	//txBuf[3] = MAX_PID;
AdrienSalle	0:32fd5a08c430	958	txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle	0:32fd5a08c430	959	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle	0:32fd5a08c430	960	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	961	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	962	txBuf[7] = playtime; // Playtime
AdrienSalle	0:32fd5a08c430	963	txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	964	txBuf[9] =(position & 0xFF00) >> 8; // position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	965	txBuf[10] = POS_MODE \| setLED; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	966	txBuf[11] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	967	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	968	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	969	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
AdrienSalle	0:32fd5a08c430	970	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	971	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	972	//txPacket(12, txBuf);
AdrienSalle	0:32fd5a08c430	973	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	974	for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle	0:32fd5a08c430	975	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	976	serial1.putc(txBuf[i]);
kyxstark	6:81733a7b69e9	977	//pc.printf("%d/",txBuf[i]);
AdrienSalle	0:32fd5a08c430	978	wait_us(100);
AdrienSalle	0:32fd5a08c430	979	}
AdrienSalle	0:32fd5a08c430	980	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	981	for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle	0:32fd5a08c430	982	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	983	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	984
AdrienSalle	0:32fd5a08c430	985	wait_us(100);
AdrienSalle	0:32fd5a08c430	986	}
AdrienSalle	0:32fd5a08c430	987	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	988	for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle	0:32fd5a08c430	989	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	990	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	991	wait_us(100);
AdrienSalle	0:32fd5a08c430	992	}
AdrienSalle	0:32fd5a08c430	993	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	994	for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle	0:32fd5a08c430	995	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	996	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	997	wait_us(100);
AdrienSalle	0:32fd5a08c430	998	}
AdrienSalle	0:32fd5a08c430	999	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1000	for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle	0:32fd5a08c430	1001	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1002	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1003	wait_us(100);
AdrienSalle	0:32fd5a08c430	1004	}
AdrienSalle	0:32fd5a08c430	1005	}
AdrienSalle	0:32fd5a08c430	1006	wait(tempo);
AdrienSalle	0:32fd5a08c430	1007	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1008	}
AdrienSalle	0:32fd5a08c430	1009	//-------------fonction de controle de vitesse pour un seul servo---------------
AdrienSalle	0:32fd5a08c430	1010
AdrienSalle	0:32fd5a08c430	1011	void velocityControl(uint8_t id, int16_t speed, uint8_t setLED, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1012	//
AdrienSalle	0:32fd5a08c430	1013	//id ==> id du servo à déplacer
AdrienSalle	0:32fd5a08c430	1014	//speed ==> vitesse (sans dec)
AdrienSalle	0:32fd5a08c430	1015	//setLED ==> LED à allumer
AdrienSalle	0:32fd5a08c430	1016	//numero_serial ==> numéro de la liaison série
AdrienSalle	0:32fd5a08c430	1017	//
AdrienSalle	0:32fd5a08c430	1018	{
AdrienSalle	0:32fd5a08c430	1019	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1020	if (speed > 1023 \|\| speed < -1023) return;
AdrienSalle	0:32fd5a08c430	1021	uint8_t txBuf[12];
AdrienSalle	0:32fd5a08c430	1022	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1023	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1024	txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
AdrienSalle	0:32fd5a08c430	1025	txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle	0:32fd5a08c430	1026	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle	0:32fd5a08c430	1027	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	1028	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	1029	txBuf[7] = 0; // Playtime, unmeaningful in turn mode
AdrienSalle	0:32fd5a08c430	1030	if (speed >= 0) { //On gère la vitesse positive
AdrienSalle	0:32fd5a08c430	1031	txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	1032	txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	1033	} else if(speed < 0) { //On gère la vitesse négative (voir pg.48 de la documentation herkulex)
AdrienSalle	0:32fd5a08c430	1034	speed= abs(speed);
AdrienSalle	0:32fd5a08c430	1035	txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	1036	txBuf[9] =((speed\|0x4000) & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	1037	}
AdrienSalle	0:32fd5a08c430	1038
AdrienSalle	0:32fd5a08c430	1039	txBuf[10] = TURN_MODE \| setLED; // Turn Mode and LED on/off
AdrienSalle	0:32fd5a08c430	1040	txBuf[11] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1041	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	1042	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	1043	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1044	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	1045	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1046	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1047	txPacket(12, txBuf,1);
AdrienSalle	0:32fd5a08c430	1048	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1049	txPacket(12, txBuf,2);
AdrienSalle	0:32fd5a08c430	1050	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1051	txPacket(12, txBuf,3);
AdrienSalle	0:32fd5a08c430	1052	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1053	txPacket(12, txBuf,4);
AdrienSalle	0:32fd5a08c430	1054	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1055	txPacket(12, txBuf,5);
AdrienSalle	0:32fd5a08c430	1056	}
AdrienSalle	0:32fd5a08c430	1057	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1058	}
AdrienSalle	0:32fd5a08c430	1059	//--------------------Compteur de tour------------------------------------------------------------------------
AdrienSalle	0:32fd5a08c430	1060	//c'est un compteur de tour (CQFD)
AdrienSalle	0:32fd5a08c430	1061	void compteTour(int ID,int16_t speed,uint8_t tour, uint16_t position,uint8_t setLED,uint8_t serial)
AdrienSalle	0:32fd5a08c430	1062	//
AdrienSalle	0:32fd5a08c430	1063	//id ==> id du servo à déplacer
AdrienSalle	0:32fd5a08c430	1064	//speed ==> vitesse
AdrienSalle	0:32fd5a08c430	1065	//tour ==> nombre de tour à effectuer
AdrienSalle	0:32fd5a08c430	1066	//position ==> position finale
AdrienSalle	0:32fd5a08c430	1067	//setLED ==> LED à allumer
AdrienSalle	0:32fd5a08c430	1068	//numero_serial ==> numéro de la liaison série
AdrienSalle	0:32fd5a08c430	1069	//
AdrienSalle	0:32fd5a08c430	1070	//
marwanesaich	12:562aa318952c	1071
marwanesaich	12:562aa318952c	1072	{
marwanesaich	12:562aa318952c	1073	int etat =0;
marwanesaich	12:562aa318952c	1074	int flagTour=0;
marwanesaich	12:562aa318952c	1075	int flagDernierTour = 0;
marwanesaich	12:562aa318952c	1076	int end = 0;
maximilienlv	14:a6e5d1ce2133	1077	int posAct, posCible, pos;
marwanesaich	12:562aa318952c	1078	servo_interrupt_en();
marwanesaich	12:562aa318952c	1079	clear(ID,serial);
marwanesaich	12:562aa318952c	1080	setTorque(ID, TORQUE_ON,serial);
marwanesaich	12:562aa318952c	1081
marwanesaich	12:562aa318952c	1082	posCible=getPos(ID,serial);//position;
marwanesaich	12:562aa318952c	1083
marwanesaich	13:050a0cb110b5	1084	//pc.printf("\nPos depart :%d",posCible);
marwanesaich	12:562aa318952c	1085	velocityControl(ID,speed,setLED,serial);
marwanesaich	12:562aa318952c	1086	wait_ms(100);
marwanesaich	13:050a0cb110b5	1087	if(tour == 0){
marwanesaich	13:050a0cb110b5	1088	etat = 2;
marwanesaich	13:050a0cb110b5	1089	tour = 1;
marwanesaich	13:050a0cb110b5	1090	}
marwanesaich	13:050a0cb110b5	1091
marwanesaich	12:562aa318952c	1092
marwanesaich	12:562aa318952c	1093	if(speed > 0) {
marwanesaich	12:562aa318952c	1094	while(end != 1) {
marwanesaich	12:562aa318952c	1095	switch (etat) {
marwanesaich	12:562aa318952c	1096	case 0 :
marwanesaich	12:562aa318952c	1097	pos = getPos(ID,serial);
marwanesaich	12:562aa318952c	1098	posAct = pos - posCible;
marwanesaich	12:562aa318952c	1099	if(posAct<0){
marwanesaich	13:050a0cb110b5	1100	//pc.printf("\n0 pos :%d",pos);
marwanesaich	12:562aa318952c	1101	clear(ID,serial);
marwanesaich	12:562aa318952c	1102	setTorque(ID, TORQUE_ON,serial);
marwanesaich	12:562aa318952c	1103	etat=1;
marwanesaich	12:562aa318952c	1104	}
marwanesaich	12:562aa318952c	1105
marwanesaich	12:562aa318952c	1106	break;
marwanesaich	12:562aa318952c	1107	case 1 :
marwanesaich	12:562aa318952c	1108	pos = getPos(ID,serial);
marwanesaich	12:562aa318952c	1109	velocityControl(ID,speed,RLED_ON,serial);
marwanesaich	12:562aa318952c	1110	posAct = posCible - pos;
marwanesaich	12:562aa318952c	1111	if (posAct < 0 ){
marwanesaich	13:050a0cb110b5	1112	//pc.printf("\n1 pos :%d",pos);
marwanesaich	12:562aa318952c	1113	etat = 2;
marwanesaich	12:562aa318952c	1114	}
marwanesaich	12:562aa318952c	1115	break;
marwanesaich	12:562aa318952c	1116
marwanesaich	12:562aa318952c	1117	case 2 :
marwanesaich	12:562aa318952c	1118
marwanesaich	12:562aa318952c	1119	clear(ID,serial);
marwanesaich	12:562aa318952c	1120	setTorque(ID, TORQUE_ON,serial);
marwanesaich	12:562aa318952c	1121
marwanesaich	12:562aa318952c	1122	if (flagTour == tour-1) {
marwanesaich	12:562aa318952c	1123	if (getPos(ID,serial) > position && !flagDernierTour){
marwanesaich	13:050a0cb110b5	1124	//pc.printf("\nTour sup");
marwanesaich	12:562aa318952c	1125	// flagTour++;
marwanesaich	12:562aa318952c	1126	flagDernierTour =1;
marwanesaich	12:562aa318952c	1127	etat =0;
marwanesaich	12:562aa318952c	1128	posCible = position;
marwanesaich	12:562aa318952c	1129	}else{
marwanesaich	13:050a0cb110b5	1130	//pc.printf("\ngoto pos finale");
marwanesaich	12:562aa318952c	1131	velocityControl(ID,0,setLED,serial);
marwanesaich	12:562aa318952c	1132	positionControl(ID,position,1,setLED,serial);
marwanesaich	12:562aa318952c	1133	end = 1;
marwanesaich	12:562aa318952c	1134	}
marwanesaich	12:562aa318952c	1135	}
marwanesaich	12:562aa318952c	1136
marwanesaich	12:562aa318952c	1137	else {
marwanesaich	12:562aa318952c	1138	flagTour=flagTour+1;
marwanesaich	13:050a0cb110b5	1139	//pc.printf("\ntours execute : %d",flagTour);
marwanesaich	12:562aa318952c	1140	etat = 0;
marwanesaich	12:562aa318952c	1141	}
marwanesaich	12:562aa318952c	1142	break;
marwanesaich	12:562aa318952c	1143	}
marwanesaich	12:562aa318952c	1144	}
marwanesaich	12:562aa318952c	1145	} else if(speed < 0) {
marwanesaich	12:562aa318952c	1146	while(end != 1) {
marwanesaich	12:562aa318952c	1147	switch (etat) {
marwanesaich	12:562aa318952c	1148	case 0 :
marwanesaich	12:562aa318952c	1149	pos = getPos(ID,serial);
marwanesaich	12:562aa318952c	1150	posAct = pos - posCible;
marwanesaich	12:562aa318952c	1151	if(posAct>0){
marwanesaich	13:050a0cb110b5	1152	//pc.printf("\n0 pos :%d",pos);
marwanesaich	12:562aa318952c	1153	clear(ID,serial);
marwanesaich	12:562aa318952c	1154	setTorque(ID, TORQUE_ON,serial);
marwanesaich	12:562aa318952c	1155	etat=1;
marwanesaich	12:562aa318952c	1156	}
marwanesaich	12:562aa318952c	1157
marwanesaich	12:562aa318952c	1158	break;
marwanesaich	12:562aa318952c	1159
marwanesaich	12:562aa318952c	1160	case 1 :
marwanesaich	12:562aa318952c	1161	pos = getPos(ID,serial);
marwanesaich	12:562aa318952c	1162	velocityControl(ID,speed,RLED_ON,serial);
marwanesaich	12:562aa318952c	1163	posAct = posCible - pos;
marwanesaich	12:562aa318952c	1164	if (posAct > 0 ){
marwanesaich	13:050a0cb110b5	1165	//pc.printf("\n1 pos :%d",pos);
marwanesaich	12:562aa318952c	1166	etat = 2;
marwanesaich	12:562aa318952c	1167	}
marwanesaich	12:562aa318952c	1168	break;
marwanesaich	12:562aa318952c	1169
marwanesaich	12:562aa318952c	1170	case 2 :
marwanesaich	12:562aa318952c	1171
marwanesaich	12:562aa318952c	1172	clear(ID,serial);
marwanesaich	12:562aa318952c	1173	setTorque(ID, TORQUE_ON,serial);
marwanesaich	12:562aa318952c	1174
marwanesaich	12:562aa318952c	1175	if (flagTour == tour-1) {
marwanesaich	12:562aa318952c	1176	if (getPos(ID,serial) < position && !flagDernierTour){
marwanesaich	13:050a0cb110b5	1177	//pc.printf("\nTour sup");
marwanesaich	12:562aa318952c	1178	// flagTour++;
marwanesaich	12:562aa318952c	1179	flagDernierTour =1;
marwanesaich	12:562aa318952c	1180	etat =0;
marwanesaich	12:562aa318952c	1181	posCible = position;
marwanesaich	12:562aa318952c	1182	}else{
marwanesaich	13:050a0cb110b5	1183	//pc.printf("\ngoto pos finale");
marwanesaich	12:562aa318952c	1184	velocityControl(ID,0,setLED,serial);
marwanesaich	12:562aa318952c	1185	positionControl(ID,position,1,setLED,serial);
marwanesaich	12:562aa318952c	1186	end = 1;
marwanesaich	12:562aa318952c	1187	}
marwanesaich	12:562aa318952c	1188	}
marwanesaich	12:562aa318952c	1189
marwanesaich	12:562aa318952c	1190	else {
marwanesaich	12:562aa318952c	1191	flagTour=flagTour+1;
marwanesaich	13:050a0cb110b5	1192	//pc.printf("\ntours execute : %d",flagTour);
marwanesaich	12:562aa318952c	1193	etat = 0;
marwanesaich	12:562aa318952c	1194	}
marwanesaich	12:562aa318952c	1195	break;
marwanesaich	12:562aa318952c	1196	}
marwanesaich	12:562aa318952c	1197	}
marwanesaich	12:562aa318952c	1198	}
marwanesaich	12:562aa318952c	1199	}
marwanesaich	12:562aa318952c	1200	/*
marwanesaich	12:562aa318952c	1201
AdrienSalle	0:32fd5a08c430	1202	{
AdrienSalle	0:32fd5a08c430	1203	int etat =0;
AdrienSalle	0:32fd5a08c430	1204	int flagTour=0;
AdrienSalle	0:32fd5a08c430	1205	int end = 0;
AdrienSalle	0:32fd5a08c430	1206	int posAct, posCible;
AdrienSalle	0:32fd5a08c430	1207	servo_interrupt_en();
AdrienSalle	0:32fd5a08c430	1208	clear(ID,serial);
AdrienSalle	0:32fd5a08c430	1209	setTorque(ID, TORQUE_ON,serial);
AdrienSalle	0:32fd5a08c430	1210
AdrienSalle	0:32fd5a08c430	1211	posCible=position;
AdrienSalle	0:32fd5a08c430	1212	velocityControl(ID,speed,setLED,serial);
AdrienSalle	0:32fd5a08c430	1213	wait_ms(100);
AdrienSalle	0:32fd5a08c430	1214
AdrienSalle	0:32fd5a08c430	1215	if(speed > 0) {
AdrienSalle	0:32fd5a08c430	1216	while(end != 1) {
AdrienSalle	0:32fd5a08c430	1217	switch (etat) {
AdrienSalle	0:32fd5a08c430	1218	case 0 :
AdrienSalle	0:32fd5a08c430	1219
AdrienSalle	0:32fd5a08c430	1220	posAct = getPos(ID,serial);
AdrienSalle	0:32fd5a08c430	1221	posAct = posAct-posCible;
kyxstark	6:81733a7b69e9	1222	//pc.printf("%d",posAct);
AdrienSalle	0:32fd5a08c430	1223	if (posAct < 0) {
AdrienSalle	0:32fd5a08c430	1224	clear(ID,serial);
AdrienSalle	0:32fd5a08c430	1225	setTorque(ID, TORQUE_ON,serial);
AdrienSalle	0:32fd5a08c430	1226	etat=1;
AdrienSalle	0:32fd5a08c430	1227	}
AdrienSalle	0:32fd5a08c430	1228	break;
AdrienSalle	0:32fd5a08c430	1229
AdrienSalle	0:32fd5a08c430	1230	case 1 :
AdrienSalle	0:32fd5a08c430	1231
AdrienSalle	0:32fd5a08c430	1232	velocityControl(ID,speed,RLED_ON,serial);
AdrienSalle	0:32fd5a08c430	1233	posAct = getPos(ID,serial);
AdrienSalle	0:32fd5a08c430	1234	posAct = posCible-posAct;
AdrienSalle	0:32fd5a08c430	1235	if (posAct < 0) etat = 2;
AdrienSalle	0:32fd5a08c430	1236	break;
AdrienSalle	0:32fd5a08c430	1237
AdrienSalle	0:32fd5a08c430	1238	case 2 :
AdrienSalle	0:32fd5a08c430	1239
AdrienSalle	0:32fd5a08c430	1240	clear(ID,serial);
AdrienSalle	0:32fd5a08c430	1241	setTorque(ID, TORQUE_ON,serial);
marwanesaich	9:0f09e3ee52e9	1242	if (flagTour == tour-1 \| tour == 0) {
AdrienSalle	0:32fd5a08c430	1243	velocityControl(ID,0,setLED,serial);
AdrienSalle	0:32fd5a08c430	1244	positionControl(ID,posCible,1,setLED,serial);
AdrienSalle	0:32fd5a08c430	1245	end = 1;
AdrienSalle	0:32fd5a08c430	1246
AdrienSalle	0:32fd5a08c430	1247	} else {
AdrienSalle	0:32fd5a08c430	1248	flagTour=flagTour+1;
AdrienSalle	0:32fd5a08c430	1249	etat = 0;
AdrienSalle	0:32fd5a08c430	1250	}
AdrienSalle	0:32fd5a08c430	1251	break;
AdrienSalle	0:32fd5a08c430	1252	}
AdrienSalle	0:32fd5a08c430	1253	}
AdrienSalle	0:32fd5a08c430	1254	} else if(speed < 0) {
AdrienSalle	0:32fd5a08c430	1255	while(end != 1) {
AdrienSalle	0:32fd5a08c430	1256	switch (etat) {
AdrienSalle	0:32fd5a08c430	1257	case 0 :
AdrienSalle	0:32fd5a08c430	1258
AdrienSalle	0:32fd5a08c430	1259	posAct = getPos(ID,serial);
AdrienSalle	0:32fd5a08c430	1260	posAct = posCible-posAct;
kyxstark	6:81733a7b69e9	1261	//pc.printf("%d",posAct);
AdrienSalle	0:32fd5a08c430	1262	if (posAct < 0) {
AdrienSalle	0:32fd5a08c430	1263	clear(ID,serial);
AdrienSalle	0:32fd5a08c430	1264	setTorque(ID, TORQUE_ON,serial);
AdrienSalle	0:32fd5a08c430	1265	etat=1;
AdrienSalle	0:32fd5a08c430	1266	}
AdrienSalle	0:32fd5a08c430	1267	break;
AdrienSalle	0:32fd5a08c430	1268
AdrienSalle	0:32fd5a08c430	1269	case 1 :
AdrienSalle	0:32fd5a08c430	1270
AdrienSalle	0:32fd5a08c430	1271	velocityControl(ID,speed,RLED_ON,serial);
AdrienSalle	0:32fd5a08c430	1272	posAct = getPos(ID,serial);
AdrienSalle	0:32fd5a08c430	1273	posAct = posAct-posCible;
AdrienSalle	0:32fd5a08c430	1274	if (posAct < 0) etat = 2;
AdrienSalle	0:32fd5a08c430	1275	break;
AdrienSalle	0:32fd5a08c430	1276
AdrienSalle	0:32fd5a08c430	1277	case 2 :
AdrienSalle	0:32fd5a08c430	1278
AdrienSalle	0:32fd5a08c430	1279	clear(ID,serial);
AdrienSalle	0:32fd5a08c430	1280	setTorque(ID, TORQUE_ON,serial);
marwanesaich	9:0f09e3ee52e9	1281	if (flagTour == tour-1 \| tour == 0) {
AdrienSalle	0:32fd5a08c430	1282	velocityControl(ID,0,setLED,serial);
AdrienSalle	0:32fd5a08c430	1283	positionControl(ID,posCible,1,setLED,serial);
AdrienSalle	0:32fd5a08c430	1284	end =1;
AdrienSalle	0:32fd5a08c430	1285	} else {
AdrienSalle	0:32fd5a08c430	1286	flagTour=flagTour+1;
AdrienSalle	0:32fd5a08c430	1287	etat = 0;
AdrienSalle	0:32fd5a08c430	1288	}
AdrienSalle	0:32fd5a08c430	1289	break;
AdrienSalle	0:32fd5a08c430	1290	}
AdrienSalle	0:32fd5a08c430	1291	}
AdrienSalle	0:32fd5a08c430	1292	}
marwanesaich	12:562aa318952c	1293	}*/
AdrienSalle	0:32fd5a08c430	1294
AdrienSalle	0:32fd5a08c430	1295	//--------------------fonction d'acquis d'etat d'un servo-----------------------
AdrienSalle	0:32fd5a08c430	1296	int8_t getStatus(uint8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1297	//
AdrienSalle	0:32fd5a08c430	1298	// renvoi l'état du servomoteur (doc pg 39)
AdrienSalle	0:32fd5a08c430	1299	//
AdrienSalle	0:32fd5a08c430	1300	//id ==> Id du servo concerné
AdrienSalle	0:32fd5a08c430	1301	//numero-serial ==> numéro de la liaison série du servo
AdrienSalle	0:32fd5a08c430	1302	//
AdrienSalle	0:32fd5a08c430	1303	{
AdrienSalle	0:32fd5a08c430	1304	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1305	uint8_t status;
AdrienSalle	0:32fd5a08c430	1306	uint8_t txBuf[7];
AdrienSalle	0:32fd5a08c430	1307	size_reponse = 9;
AdrienSalle	0:32fd5a08c430	1308
AdrienSalle	0:32fd5a08c430	1309	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1310	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1311	txBuf[2] = MIN_PACKET_SIZE; // Packet Size
AdrienSalle	0:32fd5a08c430	1312	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1313	txBuf[4] = CMD_STAT; // Status Error, Status Detail request
AdrienSalle	0:32fd5a08c430	1314	// Check Sum1 and Check Sum2
AdrienSalle	0:32fd5a08c430	1315	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1316	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	1317
AdrienSalle	0:32fd5a08c430	1318	uint8_t rxBuf[9];
AdrienSalle	0:32fd5a08c430	1319
AdrienSalle	0:32fd5a08c430	1320	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1321	for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle	0:32fd5a08c430	1322	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1323	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1324	wait_us(100);
AdrienSalle	0:32fd5a08c430	1325	}
AdrienSalle	0:32fd5a08c430	1326	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1327	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	1328	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	1329	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	1330	////////////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	1331	}
AdrienSalle	0:32fd5a08c430	1332	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1333	valueserial1=0;
AdrienSalle	0:32fd5a08c430	1334	}
AdrienSalle	0:32fd5a08c430	1335	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1336	for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle	0:32fd5a08c430	1337	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1338	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1339	wait_us(100);
AdrienSalle	0:32fd5a08c430	1340	}
AdrienSalle	0:32fd5a08c430	1341	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1342	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	1343	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	1344	rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	1345	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	1346	}
AdrienSalle	0:32fd5a08c430	1347	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1348	valueserial2=0;
AdrienSalle	0:32fd5a08c430	1349	}
AdrienSalle	0:32fd5a08c430	1350	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1351	for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle	0:32fd5a08c430	1352	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1353	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1354	wait_us(100);
AdrienSalle	0:32fd5a08c430	1355	}
AdrienSalle	0:32fd5a08c430	1356	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1357	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	1358	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	1359	rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	1360	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	1361	}
AdrienSalle	0:32fd5a08c430	1362	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	1363	valueserial3=0;
AdrienSalle	0:32fd5a08c430	1364	}
AdrienSalle	0:32fd5a08c430	1365	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1366	for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle	0:32fd5a08c430	1367	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1368	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1369	wait_us(100);
AdrienSalle	0:32fd5a08c430	1370	}
AdrienSalle	0:32fd5a08c430	1371	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1372	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	1373	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	1374	rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	1375	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	1376	}
AdrienSalle	0:32fd5a08c430	1377	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	1378	valueserial4=0;
AdrienSalle	0:32fd5a08c430	1379	}
AdrienSalle	0:32fd5a08c430	1380	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1381	for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle	0:32fd5a08c430	1382	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1383	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1384	wait_us(100);
AdrienSalle	0:32fd5a08c430	1385	}
AdrienSalle	0:32fd5a08c430	1386	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1387	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	1388	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	1389	rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	1390	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	1391	}
AdrienSalle	0:32fd5a08c430	1392	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	1393	valueserial5=0;
AdrienSalle	0:32fd5a08c430	1394	}
AdrienSalle	0:32fd5a08c430	1395	}
AdrienSalle	0:32fd5a08c430	1396
AdrienSalle	0:32fd5a08c430	1397	// Checksum1
AdrienSalle	0:32fd5a08c430	1398	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1399	if (chksum1 != rxBuf[5]) {
AdrienSalle	0:32fd5a08c430	1400	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1401	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1402	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1403	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1404	}
AdrienSalle	0:32fd5a08c430	1405	return -1;
AdrienSalle	0:32fd5a08c430	1406	}
AdrienSalle	0:32fd5a08c430	1407	// Checksum2
AdrienSalle	0:32fd5a08c430	1408	uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle	0:32fd5a08c430	1409	if (chksum2 != rxBuf[6]) {
AdrienSalle	0:32fd5a08c430	1410	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1411	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1412	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1413	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1414	}
AdrienSalle	0:32fd5a08c430	1415	return -1;
AdrienSalle	0:32fd5a08c430	1416	}
AdrienSalle	0:32fd5a08c430	1417	status = rxBuf[7]; // Status Error
AdrienSalle	0:32fd5a08c430	1418	//status = rxBuf[8]; // Status Detail
AdrienSalle	0:32fd5a08c430	1419
AdrienSalle	0:32fd5a08c430	1420
AdrienSalle	0:32fd5a08c430	1421	return status;
AdrienSalle	0:32fd5a08c430	1422	}
AdrienSalle	0:32fd5a08c430	1423	//------------------fonction pour lire la position actuelle-----------------------
AdrienSalle	0:32fd5a08c430	1424	int16_t getPos(uint8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1425	//
AdrienSalle	0:32fd5a08c430	1426	//renvoie la position d'un servo
AdrienSalle	0:32fd5a08c430	1427	//
AdrienSalle	0:32fd5a08c430	1428	//!!!ne pas oublier d'utiliser servo_interrupt_en();!!!
AdrienSalle	0:32fd5a08c430	1429	//
AdrienSalle	0:32fd5a08c430	1430	//id ==> id d'un servomoteur
AdrienSalle	0:32fd5a08c430	1431	//numero_serial==> numéro de la liaison série du servo
AdrienSalle	0:32fd5a08c430	1432	//
AdrienSalle	0:32fd5a08c430	1433	{
AdrienSalle	0:32fd5a08c430	1434	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1435	uint16_t position = 0;
AdrienSalle	0:32fd5a08c430	1436	uint8_t txBuf[9];
AdrienSalle	0:32fd5a08c430	1437	size_reponse = 13;
AdrienSalle	0:32fd5a08c430	1438
AdrienSalle	0:32fd5a08c430	1439	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1440	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1441	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle	0:32fd5a08c430	1442	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1443	txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle	0:32fd5a08c430	1444	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	1445	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	1446	txBuf[7] = RAM_CALIBRATED_POSITION; // Address
AdrienSalle	0:32fd5a08c430	1447	txBuf[8] = BYTE2;
AdrienSalle	0:32fd5a08c430	1448	// Check Sum1 and Check Sum2
AdrienSalle	0:32fd5a08c430	1449	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1450	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	1451
AdrienSalle	0:32fd5a08c430	1452	uint8_t rxBuf[13];
AdrienSalle	0:32fd5a08c430	1453
AdrienSalle	0:32fd5a08c430	1454	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1455	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1456	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1457	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1458	wait_us(100);
AdrienSalle	0:32fd5a08c430	1459	}
AdrienSalle	0:32fd5a08c430	1460	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1461	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	1462	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	1463	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	1464	//pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	1465	}
AdrienSalle	0:32fd5a08c430	1466	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1467	valueserial1=0;
AdrienSalle	0:32fd5a08c430	1468	}
AdrienSalle	0:32fd5a08c430	1469	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1470	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1471	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1472	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1473	wait_us(100);
AdrienSalle	0:32fd5a08c430	1474	}
AdrienSalle	0:32fd5a08c430	1475	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1476	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	1477	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	1478	rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	1479	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	1480	}
AdrienSalle	0:32fd5a08c430	1481	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1482	valueserial2=0;
AdrienSalle	0:32fd5a08c430	1483	}
AdrienSalle	0:32fd5a08c430	1484	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1485	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1486	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1487	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1488	wait_us(100);
AdrienSalle	0:32fd5a08c430	1489	}
AdrienSalle	0:32fd5a08c430	1490	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1491	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	1492	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	1493	rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	1494	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	1495	}
AdrienSalle	0:32fd5a08c430	1496	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	1497	valueserial3=0;
AdrienSalle	0:32fd5a08c430	1498	}
AdrienSalle	0:32fd5a08c430	1499	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1500	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1501	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1502	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1503	wait_us(100);
AdrienSalle	0:32fd5a08c430	1504	}
AdrienSalle	0:32fd5a08c430	1505	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1506	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	1507	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	1508	rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	1509	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	1510	}
AdrienSalle	0:32fd5a08c430	1511	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	1512	valueserial4=0;
AdrienSalle	0:32fd5a08c430	1513	}
AdrienSalle	0:32fd5a08c430	1514	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1515	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1516	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1517	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1518	wait_us(100);
AdrienSalle	0:32fd5a08c430	1519	}
AdrienSalle	0:32fd5a08c430	1520	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1521	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	1522	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	1523	rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	1524	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	1525	}
AdrienSalle	0:32fd5a08c430	1526	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	1527	valueserial5=0;
AdrienSalle	0:32fd5a08c430	1528	}
AdrienSalle	0:32fd5a08c430	1529	}
AdrienSalle	0:32fd5a08c430	1530
AdrienSalle	0:32fd5a08c430	1531	// Checksum1
AdrienSalle	0:32fd5a08c430	1532	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1533	if (chksum1 != rxBuf[5]) {
AdrienSalle	0:32fd5a08c430	1534	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1535	pc->printf("Checksum1 fault\n");
AdrienSalle	0:32fd5a08c430	1536	#endif*/
AdrienSalle	0:32fd5a08c430	1537	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1538	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1539	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1540	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1541	}
AdrienSalle	0:32fd5a08c430	1542	return -1;
AdrienSalle	0:32fd5a08c430	1543	}
AdrienSalle	0:32fd5a08c430	1544	// Checksum2
AdrienSalle	0:32fd5a08c430	1545	uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle	0:32fd5a08c430	1546	if (chksum2 != rxBuf[6]) {
AdrienSalle	0:32fd5a08c430	1547	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1548	pc->printf("Checksum2 fault\n");
AdrienSalle	0:32fd5a08c430	1549	#endif*/
AdrienSalle	0:32fd5a08c430	1550	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1551	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1552	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1553	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1554	}
AdrienSalle	0:32fd5a08c430	1555	return -1;
AdrienSalle	0:32fd5a08c430	1556	}
AdrienSalle	0:32fd5a08c430	1557	position = ((rxBuf[10]&0x03)<<8) \| rxBuf[9];
AdrienSalle	0:32fd5a08c430	1558
AdrienSalle	0:32fd5a08c430	1559
AdrienSalle	0:32fd5a08c430	1560	//}
AdrienSalle	0:32fd5a08c430	1561	return position;
AdrienSalle	0:32fd5a08c430	1562	}
AdrienSalle	0:32fd5a08c430	1563	//---------------fonction d'acquis d'etat de couple d'un servo------------------
AdrienSalle	0:32fd5a08c430	1564	//Obtenir la valeur du couple d'un servo
AdrienSalle	0:32fd5a08c430	1565	int8_t Get_Torque(int8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1566	//
AdrienSalle	0:32fd5a08c430	1567	//id ==> id du servomoteur sur la liaison série
AdrienSalle	0:32fd5a08c430	1568	//numero_serial ==> numéro de la liaison série sur laquelle se trouve le servomoteur
AdrienSalle	0:32fd5a08c430	1569	//
AdrienSalle	0:32fd5a08c430	1570	{
AdrienSalle	0:32fd5a08c430	1571	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1572	uint8_t txBuf[9];
AdrienSalle	0:32fd5a08c430	1573	int8_t Tor = 0;
AdrienSalle	0:32fd5a08c430	1574
AdrienSalle	0:32fd5a08c430	1575	uint8_t iv=0;
AdrienSalle	0:32fd5a08c430	1576	for(iv=0; iv<20; iv++) {
AdrienSalle	0:32fd5a08c430	1577	rx2[iv] = 0;
AdrienSalle	0:32fd5a08c430	1578	}
AdrienSalle	0:32fd5a08c430	1579
AdrienSalle	0:32fd5a08c430	1580	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1581	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1582	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle	0:32fd5a08c430	1583	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1584	txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle	0:32fd5a08c430	1585	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	1586	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	1587	txBuf[7] = RAM_TORQUE_CONTROL;
AdrienSalle	0:32fd5a08c430	1588	txBuf[8] = BYTE1; // Length
AdrienSalle	0:32fd5a08c430	1589	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;//Checksum2
AdrienSalle	0:32fd5a08c430	1590	txBuf[6] = (~txBuf[5])&0xFE; // CheckSum2
AdrienSalle	0:32fd5a08c430	1591
AdrienSalle	0:32fd5a08c430	1592	//pc.printf(" Torque ");
AdrienSalle	0:32fd5a08c430	1593
AdrienSalle	0:32fd5a08c430	1594	uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	1595
AdrienSalle	0:32fd5a08c430	1596	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1597	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1598	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1599	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1600	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1601	wait_us(100);
AdrienSalle	0:32fd5a08c430	1602	}
AdrienSalle	0:32fd5a08c430	1603
AdrienSalle	0:32fd5a08c430	1604	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1605	//uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	1606	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1607	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	1608	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	1609	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	1610	//////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	1611	}
AdrienSalle	0:32fd5a08c430	1612	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1613	valueserial1=0;
AdrienSalle	0:32fd5a08c430	1614	}
AdrienSalle	0:32fd5a08c430	1615	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1616	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1617	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1618	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1619	wait_us(100);
AdrienSalle	0:32fd5a08c430	1620	}
AdrienSalle	0:32fd5a08c430	1621	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1622	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	1623	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	1624	rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	1625	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	1626	}
AdrienSalle	0:32fd5a08c430	1627	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1628	valueserial2=0;
AdrienSalle	0:32fd5a08c430	1629	}
AdrienSalle	0:32fd5a08c430	1630	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1631	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1632	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1633	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1634	wait_us(100);
AdrienSalle	0:32fd5a08c430	1635	}
AdrienSalle	0:32fd5a08c430	1636	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1637	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	1638	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	1639	rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	1640	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	1641	}
AdrienSalle	0:32fd5a08c430	1642	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	1643	valueserial3=0;
AdrienSalle	0:32fd5a08c430	1644	}
AdrienSalle	0:32fd5a08c430	1645	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1646	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1647	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1648	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1649	wait_us(100);
AdrienSalle	0:32fd5a08c430	1650	}
AdrienSalle	0:32fd5a08c430	1651	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1652	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	1653	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	1654	rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	1655	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	1656	}
AdrienSalle	0:32fd5a08c430	1657	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	1658	valueserial4=0;
AdrienSalle	0:32fd5a08c430	1659	}
AdrienSalle	0:32fd5a08c430	1660	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1661	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1662	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1663	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1664	wait_us(100);
AdrienSalle	0:32fd5a08c430	1665	}
AdrienSalle	0:32fd5a08c430	1666	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1667	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	1668	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	1669	rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	1670	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	1671	}
AdrienSalle	0:32fd5a08c430	1672	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	1673	valueserial5=0;
AdrienSalle	0:32fd5a08c430	1674	}
AdrienSalle	0:32fd5a08c430	1675	}
AdrienSalle	0:32fd5a08c430	1676
AdrienSalle	0:32fd5a08c430	1677	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1678	if (chksum1 != rxBuf[5]) {
AdrienSalle	0:32fd5a08c430	1679	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1680	pc->printf("Checksum1 fault\n");
AdrienSalle	0:32fd5a08c430	1681	#endif*/
AdrienSalle	0:32fd5a08c430	1682	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1683	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1684	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1685	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1686	}
AdrienSalle	0:32fd5a08c430	1687	return -1;
AdrienSalle	0:32fd5a08c430	1688	}
AdrienSalle	0:32fd5a08c430	1689	// Checksum2
AdrienSalle	0:32fd5a08c430	1690	uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle	0:32fd5a08c430	1691	if (chksum2 != rxBuf[6]) {
AdrienSalle	0:32fd5a08c430	1692	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1693	pc->printf("Checksum2 fault\n");
AdrienSalle	0:32fd5a08c430	1694	#endif*/
AdrienSalle	0:32fd5a08c430	1695	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1696	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1697	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1698	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1699	}
AdrienSalle	0:32fd5a08c430	1700	return -1;
AdrienSalle	0:32fd5a08c430	1701	}
AdrienSalle	0:32fd5a08c430	1702	Tor = rxBuf[9];
AdrienSalle	0:32fd5a08c430	1703	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1704	pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle	0:32fd5a08c430	1705	#endif*/
AdrienSalle	0:32fd5a08c430	1706	//}
AdrienSalle	0:32fd5a08c430	1707	return Tor;
AdrienSalle	0:32fd5a08c430	1708	}
AdrienSalle	0:32fd5a08c430	1709	//---------------fonction pour lire le temperature max pour un servo------------
AdrienSalle	0:32fd5a08c430	1710	//obtenir la valeur de température maximum tolérée par le servomoteur
AdrienSalle	0:32fd5a08c430	1711	int8_t Get_Temperature_MAX(int8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1712	//
AdrienSalle	0:32fd5a08c430	1713	//id ==> id du servomoteur sur la liaison série
AdrienSalle	0:32fd5a08c430	1714	//numero_serial ==> numéro de la liaison série sur laquelle se trouve le servomoteur
AdrienSalle	0:32fd5a08c430	1715	//
AdrienSalle	0:32fd5a08c430	1716	{
AdrienSalle	0:32fd5a08c430	1717	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1718	uint8_t txBuf[9];
AdrienSalle	0:32fd5a08c430	1719	int8_t tempeMAX = 0;
AdrienSalle	0:32fd5a08c430	1720
AdrienSalle	0:32fd5a08c430	1721	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1722	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1723	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle	0:32fd5a08c430	1724	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1725	txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle	0:32fd5a08c430	1726	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	1727	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	1728	txBuf[7] = RAM_MAX_TEMPERATURE;
AdrienSalle	0:32fd5a08c430	1729	txBuf[8] = BYTE1; // Length
AdrienSalle	0:32fd5a08c430	1730	// Check Sum1 and Check Sum2
AdrienSalle	0:32fd5a08c430	1731	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1732	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	1733
AdrienSalle	0:32fd5a08c430	1734	//pc.printf(" tempeMAX ");
AdrienSalle	0:32fd5a08c430	1735
AdrienSalle	0:32fd5a08c430	1736	uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	1737
AdrienSalle	0:32fd5a08c430	1738	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1739	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1740	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1741	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1742	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1743	wait_us(100);
AdrienSalle	0:32fd5a08c430	1744	}
AdrienSalle	0:32fd5a08c430	1745
AdrienSalle	0:32fd5a08c430	1746	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1747	//uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	1748	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1749	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	1750	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	1751	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	1752	//////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	1753	}
AdrienSalle	0:32fd5a08c430	1754	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1755	valueserial1=0;
AdrienSalle	0:32fd5a08c430	1756	}
AdrienSalle	0:32fd5a08c430	1757	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1758	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1759	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1760	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1761	wait_us(100);
AdrienSalle	0:32fd5a08c430	1762	}
AdrienSalle	0:32fd5a08c430	1763	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1764	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	1765	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	1766	rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	1767	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	1768	}
AdrienSalle	0:32fd5a08c430	1769	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1770	valueserial2=0;
AdrienSalle	0:32fd5a08c430	1771	}
AdrienSalle	0:32fd5a08c430	1772	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1773	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1774	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1775	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1776	wait_us(100);
AdrienSalle	0:32fd5a08c430	1777	}
AdrienSalle	0:32fd5a08c430	1778	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1779	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	1780	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	1781	rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	1782	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	1783	}
AdrienSalle	0:32fd5a08c430	1784	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	1785	valueserial3=0;
AdrienSalle	0:32fd5a08c430	1786	}
AdrienSalle	0:32fd5a08c430	1787	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1788	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1789	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1790	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1791	wait_us(100);
AdrienSalle	0:32fd5a08c430	1792	}
AdrienSalle	0:32fd5a08c430	1793	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1794	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	1795	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	1796	rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	1797	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	1798	}
AdrienSalle	0:32fd5a08c430	1799	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	1800	valueserial4=0;
AdrienSalle	0:32fd5a08c430	1801	}
AdrienSalle	0:32fd5a08c430	1802	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1803	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	1804	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1805	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1806	wait_us(100);
AdrienSalle	0:32fd5a08c430	1807	}
AdrienSalle	0:32fd5a08c430	1808	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1809	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	1810	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	1811	rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	1812	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	1813	}
AdrienSalle	0:32fd5a08c430	1814	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	1815	valueserial5=0;
AdrienSalle	0:32fd5a08c430	1816	}
AdrienSalle	0:32fd5a08c430	1817	}
AdrienSalle	0:32fd5a08c430	1818
AdrienSalle	0:32fd5a08c430	1819	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1820	if (chksum1 != rxBuf[5]) {
AdrienSalle	0:32fd5a08c430	1821	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1822	pc->printf("Checksum1 fault\n");
AdrienSalle	0:32fd5a08c430	1823	#endif*/
AdrienSalle	0:32fd5a08c430	1824	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1825	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1826	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1827	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1828	}
AdrienSalle	0:32fd5a08c430	1829	return -1;
AdrienSalle	0:32fd5a08c430	1830	}
AdrienSalle	0:32fd5a08c430	1831	// Checksum2
AdrienSalle	0:32fd5a08c430	1832	uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle	0:32fd5a08c430	1833	if (chksum2 != rxBuf[6]) {
AdrienSalle	0:32fd5a08c430	1834	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1835	pc->printf("Checksum2 fault\n");
AdrienSalle	0:32fd5a08c430	1836	#endif*/
AdrienSalle	0:32fd5a08c430	1837	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1838	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	1839	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1840	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	1841	}
AdrienSalle	0:32fd5a08c430	1842	return -1;
AdrienSalle	0:32fd5a08c430	1843	}
AdrienSalle	0:32fd5a08c430	1844	tempeMAX = rxBuf[9];
AdrienSalle	0:32fd5a08c430	1845	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	1846	pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle	0:32fd5a08c430	1847	#endif*/
AdrienSalle	0:32fd5a08c430	1848	//}
AdrienSalle	0:32fd5a08c430	1849	return tempeMAX;
AdrienSalle	0:32fd5a08c430	1850	}
AdrienSalle	0:32fd5a08c430	1851	//--------fonction de controle de position pour deux servo(same playtime)-------
AdrienSalle	0:32fd5a08c430	1852	//permet de déplacer deux servomoteurs sur la même liaison série avec le même temps d'execution
AdrienSalle	0:32fd5a08c430	1853	void positionControl_Mul_ensemble(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t setLED2, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1854	//
AdrienSalle	0:32fd5a08c430	1855	//id
AdrienSalle	0:32fd5a08c430	1856	//
AdrienSalle	0:32fd5a08c430	1857
AdrienSalle	0:32fd5a08c430	1858	{
AdrienSalle	0:32fd5a08c430	1859	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1860	float tempo=0;
AdrienSalle	0:32fd5a08c430	1861	//if (position > 1023) return; //1002-21
AdrienSalle	0:32fd5a08c430	1862	if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
AdrienSalle	0:32fd5a08c430	1863	tempo=playtime*0.012;
AdrienSalle	0:32fd5a08c430	1864	uint8_t txBuf[16];
AdrienSalle	0:32fd5a08c430	1865	etat = pos;
AdrienSalle	0:32fd5a08c430	1866	pos_position = position;
AdrienSalle	0:32fd5a08c430	1867	pos_time = playtime;
AdrienSalle	0:32fd5a08c430	1868	pos_led = setLED;
AdrienSalle	0:32fd5a08c430	1869	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1870	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1871	txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
AdrienSalle	0:32fd5a08c430	1872	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle	0:32fd5a08c430	1873	txBuf[3] = 254; // broadcast ID
AdrienSalle	0:32fd5a08c430	1874	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle	0:32fd5a08c430	1875	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	1876	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	1877	txBuf[7] = playtime; // Playtime
AdrienSalle	0:32fd5a08c430	1878	txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	1879	txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	1880	txBuf[10] = POS_MODE \| setLED; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	1881	txBuf[11] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1882	txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	1883	txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	1884	txBuf[14] = POS_MODE \| setLED2; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	1885	txBuf[15] = id2; // Servo ID
AdrienSalle	0:32fd5a08c430	1886	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	1887	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	1888	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]^txBuf[12]^txBuf[13]^txBuf[14]^txBuf[15]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1889	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	1890	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1891
AdrienSalle	0:32fd5a08c430	1892	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1893	for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle	0:32fd5a08c430	1894	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1895	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1896	wait_us(100);
AdrienSalle	0:32fd5a08c430	1897	}
AdrienSalle	0:32fd5a08c430	1898	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1899	for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle	0:32fd5a08c430	1900	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1901	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1902	wait_us(100);
AdrienSalle	0:32fd5a08c430	1903	}
AdrienSalle	0:32fd5a08c430	1904	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1905	for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle	0:32fd5a08c430	1906	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1907	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1908	wait_us(100);
AdrienSalle	0:32fd5a08c430	1909	}
AdrienSalle	0:32fd5a08c430	1910	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1911	for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle	0:32fd5a08c430	1912	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1913	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1914	wait_us(100);
AdrienSalle	0:32fd5a08c430	1915	}
AdrienSalle	0:32fd5a08c430	1916	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1917	for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle	0:32fd5a08c430	1918	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1919	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1920	wait_us(100);
AdrienSalle	0:32fd5a08c430	1921	}
AdrienSalle	0:32fd5a08c430	1922	}
AdrienSalle	0:32fd5a08c430	1923	wait(tempo);
AdrienSalle	0:32fd5a08c430	1924	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	1925	}
AdrienSalle	0:32fd5a08c430	1926	//-----fonction de controle de position pour deux servo(different playtime)----- //a changer...
AdrienSalle	0:32fd5a08c430	1927	void positionControl_Mul_playtime_different(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t playtime2, uint8_t setLED2, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	1928	//
AdrienSalle	0:32fd5a08c430	1929	//permet de controler deux servomoteurs avec des temps d'execution différents
AdrienSalle	0:32fd5a08c430	1930	//
AdrienSalle	0:32fd5a08c430	1931	{
AdrienSalle	0:32fd5a08c430	1932	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	1933	float tempo=0;
AdrienSalle	0:32fd5a08c430	1934	//if (position > 1023) return; //1002-21
AdrienSalle	0:32fd5a08c430	1935	if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
AdrienSalle	0:32fd5a08c430	1936	if(playtime>playtime2) {
AdrienSalle	0:32fd5a08c430	1937	tempo=playtime*0.012;
AdrienSalle	0:32fd5a08c430	1938	} else if(playtime<playtime2) {
AdrienSalle	0:32fd5a08c430	1939	tempo=playtime2*0.012;
AdrienSalle	0:32fd5a08c430	1940	}
AdrienSalle	0:32fd5a08c430	1941	uint8_t txBuf[17];
AdrienSalle	0:32fd5a08c430	1942	etat = pos;
AdrienSalle	0:32fd5a08c430	1943	pos_position = position;
AdrienSalle	0:32fd5a08c430	1944	pos_time = playtime;
AdrienSalle	0:32fd5a08c430	1945	pos_led = setLED;
AdrienSalle	0:32fd5a08c430	1946	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1947	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	1948	txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
AdrienSalle	0:32fd5a08c430	1949	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle	0:32fd5a08c430	1950	txBuf[3] = 254; // broadcast ID
AdrienSalle	0:32fd5a08c430	1951	txBuf[4] = CMD_I_JOG; // Command I JOG
AdrienSalle	0:32fd5a08c430	1952	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	1953	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	1954	txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	1955	txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	1956	txBuf[9] = POS_MODE \| setLED; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	1957	txBuf[10] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	1958	txBuf[11] = playtime; // Playtime
AdrienSalle	0:32fd5a08c430	1959	txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	1960	txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	1961	txBuf[14] = POS_MODE \| setLED2; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	1962	txBuf[15] = id2; // Servo ID
AdrienSalle	0:32fd5a08c430	1963	txBuf[16] = playtime2; // Playtime
AdrienSalle	0:32fd5a08c430	1964	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	1965	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	1966	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]^txBuf[12]^txBuf[13]^txBuf[14]^txBuf[15]) & 0xFE;
AdrienSalle	0:32fd5a08c430	1967	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	1968	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	1969	//txPacket(12, txBuf);
AdrienSalle	0:32fd5a08c430	1970
AdrienSalle	0:32fd5a08c430	1971	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	1972	for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle	0:32fd5a08c430	1973	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1974	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1975	wait_us(100);
AdrienSalle	0:32fd5a08c430	1976	}
AdrienSalle	0:32fd5a08c430	1977	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	1978	for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle	0:32fd5a08c430	1979	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1980	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1981	wait_us(100);
AdrienSalle	0:32fd5a08c430	1982	}
AdrienSalle	0:32fd5a08c430	1983	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	1984	for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle	0:32fd5a08c430	1985	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1986	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1987	wait_us(100);
AdrienSalle	0:32fd5a08c430	1988	}
AdrienSalle	0:32fd5a08c430	1989	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	1990	for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle	0:32fd5a08c430	1991	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1992	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1993	wait_us(100);
AdrienSalle	0:32fd5a08c430	1994	}
AdrienSalle	0:32fd5a08c430	1995	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	1996	for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle	0:32fd5a08c430	1997	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	1998	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	1999	wait_us(100);
AdrienSalle	0:32fd5a08c430	2000	}
AdrienSalle	0:32fd5a08c430	2001	}
AdrienSalle	0:32fd5a08c430	2002
AdrienSalle	0:32fd5a08c430	2003	wait(tempo);
AdrienSalle	0:32fd5a08c430	2004	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2005	}
AdrienSalle	0:32fd5a08c430	2006	//-----fonction de controle de position pour plusieurs servo(same playtime)-----
AdrienSalle	0:32fd5a08c430	2007	void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos, uint8_t numero_serial) // uint16_t position, uint8_t setLED, uint8_t id
AdrienSalle	0:32fd5a08c430	2008	//
AdrienSalle	0:32fd5a08c430	2009	//Permet de controler tout les servos de la même liaison série avec le même temps d'execution
AdrienSalle	0:32fd5a08c430	2010	//
AdrienSalle	0:32fd5a08c430	2011	//
AdrienSalle	0:32fd5a08c430	2012	{
AdrienSalle	0:32fd5a08c430	2013	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	2014	//float tempo=0;
AdrienSalle	0:32fd5a08c430	2015	uint8_t taille = 0,i = 0,idata = 0, ipos = 0;
AdrienSalle	0:32fd5a08c430	2016	//if (position > 1023) return; //1002-21
AdrienSalle	0:32fd5a08c430	2017	if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
AdrienSalle	0:32fd5a08c430	2018	//tempo=playtime*0.012;
AdrienSalle	0:32fd5a08c430	2019	taille = 7 + 1 + 4 * nb_servo;
AdrienSalle	0:32fd5a08c430	2020	nombre_servo = nb_servo;
AdrienSalle	0:32fd5a08c430	2021	pos_time = playtime;
AdrienSalle	0:32fd5a08c430	2022	uint8_t txBuf[taille];
AdrienSalle	0:32fd5a08c430	2023	etat = pos_mul_complex;
AdrienSalle	0:32fd5a08c430	2024
AdrienSalle	0:32fd5a08c430	2025	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2026	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2027	txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size
AdrienSalle	0:32fd5a08c430	2028	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle	0:32fd5a08c430	2029	txBuf[3] = 254; // broadcast ID
AdrienSalle	0:32fd5a08c430	2030	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle	0:32fd5a08c430	2031	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	2032	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	2033	txBuf[7] = playtime; // Playtime
AdrienSalle	0:32fd5a08c430	2034
AdrienSalle	0:32fd5a08c430	2035	for(i=0; i<nb_servo; i++) {
AdrienSalle	0:32fd5a08c430	2036	txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	2037	txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	2038	position_servo_mul[ipos] = pos[ipos];
AdrienSalle	0:32fd5a08c430	2039	ipos++;
AdrienSalle	0:32fd5a08c430	2040	txBuf[10+i*4] = POS_MODE \| data[idata]; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	2041	data_servo_mul[idata] = data[idata];
AdrienSalle	0:32fd5a08c430	2042	idata++;
AdrienSalle	0:32fd5a08c430	2043	txBuf[11+i*4] = data[idata]; // Servo ID
AdrienSalle	0:32fd5a08c430	2044	data_servo_mul[idata] = data[idata];
AdrienSalle	0:32fd5a08c430	2045	idata++;
AdrienSalle	0:32fd5a08c430	2046	}
AdrienSalle	0:32fd5a08c430	2047
AdrienSalle	0:32fd5a08c430	2048	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	2049	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	2050	txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7];
AdrienSalle	0:32fd5a08c430	2051
AdrienSalle	0:32fd5a08c430	2052	for(i=1; i<(taille-7); i++) {
AdrienSalle	0:32fd5a08c430	2053	txBuf[5]=txBuf[5]^txBuf[7+i];
AdrienSalle	0:32fd5a08c430	2054	}
AdrienSalle	0:32fd5a08c430	2055	txBuf[5] = txBuf[5]& 0xFE;
AdrienSalle	0:32fd5a08c430	2056	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	2057
AdrienSalle	0:32fd5a08c430	2058	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2059	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2060	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2061	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2062	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2063	wait_us(100);
AdrienSalle	0:32fd5a08c430	2064	}
AdrienSalle	0:32fd5a08c430	2065	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2066	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2067	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2068	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2069	wait_us(100);
AdrienSalle	0:32fd5a08c430	2070	}
AdrienSalle	0:32fd5a08c430	2071	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	2072	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2073	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2074	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2075	wait_us(100);
AdrienSalle	0:32fd5a08c430	2076	}
AdrienSalle	0:32fd5a08c430	2077	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	2078	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2079	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2080	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2081	wait_us(100);
AdrienSalle	0:32fd5a08c430	2082	}
AdrienSalle	0:32fd5a08c430	2083	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	2084	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2085	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2086	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2087	wait_us(100);
AdrienSalle	0:32fd5a08c430	2088	}
AdrienSalle	0:32fd5a08c430	2089	}
AdrienSalle	0:32fd5a08c430	2090
AdrienSalle	0:32fd5a08c430	2091	/*for(uint8_t i = 0; i < taille ; i++)
AdrienSalle	0:32fd5a08c430	2092	{
AdrienSalle	0:32fd5a08c430	2093	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2094	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2095	wait_us(100);
AdrienSalle	0:32fd5a08c430	2096	}*/
AdrienSalle	0:32fd5a08c430	2097	//wait(tempo);
AdrienSalle	0:32fd5a08c430	2098	//wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2099	}
AdrienSalle	0:32fd5a08c430	2100	//--fonction de controle de position pour plusieurs servo(different playtime)---
AdrienSalle	0:32fd5a08c430	2101	void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos, uint8_t numero_serial) // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime
AdrienSalle	0:32fd5a08c430	2102	//
AdrienSalle	0:32fd5a08c430	2103	//Permet de controler tout les servos de la même liaison série avec un temps d'execution différent
AdrienSalle	0:32fd5a08c430	2104	//
AdrienSalle	0:32fd5a08c430	2105	{
AdrienSalle	0:32fd5a08c430	2106	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	2107	float tempo=0;
AdrienSalle	0:32fd5a08c430	2108	uint8_t Max_playtime = 0;
AdrienSalle	0:32fd5a08c430	2109	uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0;
AdrienSalle	0:32fd5a08c430	2110	//if (position > 1023) return; //1002-21
AdrienSalle	0:32fd5a08c430	2111	//if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
AdrienSalle	0:32fd5a08c430	2112
AdrienSalle	0:32fd5a08c430	2113	for(iplay_time=0; iplay_time<nb_servo; iplay_time++) {
AdrienSalle	0:32fd5a08c430	2114	if(Max_playtime<data[2+3*iplay_time]) {
AdrienSalle	0:32fd5a08c430	2115	Max_playtime=data[2+3*iplay_time];
AdrienSalle	0:32fd5a08c430	2116	}
AdrienSalle	0:32fd5a08c430	2117	}
AdrienSalle	0:32fd5a08c430	2118	tempo=Max_playtime*0.012;
AdrienSalle	0:32fd5a08c430	2119	taille = 7 + 5 * nb_servo;
AdrienSalle	0:32fd5a08c430	2120	nombre_servo = nb_servo;
AdrienSalle	0:32fd5a08c430	2121	uint8_t txBuf[taille];
AdrienSalle	0:32fd5a08c430	2122	etat = pos_mul_complex_different;
AdrienSalle	0:32fd5a08c430	2123
AdrienSalle	0:32fd5a08c430	2124	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2125	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2126	txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size
AdrienSalle	0:32fd5a08c430	2127	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle	0:32fd5a08c430	2128	txBuf[3] = 254; // broadcast ID
AdrienSalle	0:32fd5a08c430	2129	txBuf[4] = CMD_I_JOG; // Command I JOG (0x06)
AdrienSalle	0:32fd5a08c430	2130	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	2131	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	2132
AdrienSalle	0:32fd5a08c430	2133	for(i=0; i<nb_servo; i++) {
AdrienSalle	0:32fd5a08c430	2134	txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle	0:32fd5a08c430	2135	txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle	0:32fd5a08c430	2136	position_servo_mul_different[ipos] = pos[ipos];
AdrienSalle	0:32fd5a08c430	2137	ipos++;
AdrienSalle	0:32fd5a08c430	2138	txBuf[9+i*5] = POS_MODE \| data[idata]; // Pos Mode and LED on/off
AdrienSalle	0:32fd5a08c430	2139	data_servo_mul_different[idata] = data[idata];
AdrienSalle	0:32fd5a08c430	2140	idata++;
AdrienSalle	0:32fd5a08c430	2141	txBuf[10+i*5] = data[idata]; // Servo ID
AdrienSalle	0:32fd5a08c430	2142	data_servo_mul_different[idata] = data[idata];
AdrienSalle	0:32fd5a08c430	2143	idata++;
AdrienSalle	0:32fd5a08c430	2144	txBuf[11+i*5] = data[idata]; // Playtime
AdrienSalle	0:32fd5a08c430	2145	data_servo_mul_different[idata] = data[idata];
AdrienSalle	0:32fd5a08c430	2146	idata++;
AdrienSalle	0:32fd5a08c430	2147	}
AdrienSalle	0:32fd5a08c430	2148
AdrienSalle	0:32fd5a08c430	2149	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle	0:32fd5a08c430	2150	// Checksum2 = (~Checksum1)&0xFE
AdrienSalle	0:32fd5a08c430	2151	txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4];
AdrienSalle	0:32fd5a08c430	2152
AdrienSalle	0:32fd5a08c430	2153	for(i=1; i<(taille-6); i++) {
AdrienSalle	0:32fd5a08c430	2154	txBuf[5]=txBuf[5]^txBuf[6+i];
AdrienSalle	0:32fd5a08c430	2155	}
AdrienSalle	0:32fd5a08c430	2156	txBuf[5] = txBuf[5]& 0xFE;
AdrienSalle	0:32fd5a08c430	2157	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	2158
AdrienSalle	0:32fd5a08c430	2159	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2160	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2161	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2162	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2163	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2164	wait_us(100);
AdrienSalle	0:32fd5a08c430	2165	}
AdrienSalle	0:32fd5a08c430	2166	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2167	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2168	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2169	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2170	wait_us(100);
AdrienSalle	0:32fd5a08c430	2171	}
AdrienSalle	0:32fd5a08c430	2172	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	2173	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2174	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2175	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2176	wait_us(100);
AdrienSalle	0:32fd5a08c430	2177	}
AdrienSalle	0:32fd5a08c430	2178	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	2179	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2180	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2181	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2182	wait_us(100);
AdrienSalle	0:32fd5a08c430	2183	}
AdrienSalle	0:32fd5a08c430	2184	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	2185	for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle	0:32fd5a08c430	2186	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2187	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2188	wait_us(100);
AdrienSalle	0:32fd5a08c430	2189	}
AdrienSalle	0:32fd5a08c430	2190	}
AdrienSalle	0:32fd5a08c430	2191
AdrienSalle	0:32fd5a08c430	2192	/*for(uint8_t i = 0; i < taille ; i++)
AdrienSalle	0:32fd5a08c430	2193	{
AdrienSalle	0:32fd5a08c430	2194	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2195	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2196	wait_us(100);
AdrienSalle	0:32fd5a08c430	2197	}*/
AdrienSalle	0:32fd5a08c430	2198	wait(tempo);
AdrienSalle	0:32fd5a08c430	2199	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2200	}
AdrienSalle	0:32fd5a08c430	2201	//---------------fonction pour lire la tension minimale pour un servo----------------
AdrienSalle	0:32fd5a08c430	2202	int8_t Get_Tension_MIN(int8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	2203	{
AdrienSalle	0:32fd5a08c430	2204	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	2205	uint8_t txBuf[9];
AdrienSalle	0:32fd5a08c430	2206	int8_t tensionMIN = 0;
AdrienSalle	0:32fd5a08c430	2207
AdrienSalle	0:32fd5a08c430	2208	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2209	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2210	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle	0:32fd5a08c430	2211	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	2212	txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle	0:32fd5a08c430	2213	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	2214	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	2215	txBuf[7] = RAM_MIN_VOLTAGE;
AdrienSalle	0:32fd5a08c430	2216	txBuf[8] = BYTE1; // Length
AdrienSalle	0:32fd5a08c430	2217	// Check Sum1 and Check Sum2
AdrienSalle	0:32fd5a08c430	2218	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle	0:32fd5a08c430	2219	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	2220
AdrienSalle	0:32fd5a08c430	2221	//pc.printf(" tensionMIN ");
AdrienSalle	0:32fd5a08c430	2222
AdrienSalle	0:32fd5a08c430	2223	uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	2224
AdrienSalle	0:32fd5a08c430	2225	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2226	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2227	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2228	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2229	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2230	wait_us(100);
AdrienSalle	0:32fd5a08c430	2231	}
AdrienSalle	0:32fd5a08c430	2232
AdrienSalle	0:32fd5a08c430	2233	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2234	//uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	2235	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2236	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	2237	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	2238	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	2239	//////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	2240	}
AdrienSalle	0:32fd5a08c430	2241	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2242	valueserial1=0;
AdrienSalle	0:32fd5a08c430	2243	}
AdrienSalle	0:32fd5a08c430	2244	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2245	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2246	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2247	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2248	wait_us(100);
AdrienSalle	0:32fd5a08c430	2249	}
AdrienSalle	0:32fd5a08c430	2250	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2251	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	2252	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	2253	rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	2254	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	2255	}
AdrienSalle	0:32fd5a08c430	2256	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	2257	valueserial2=0;
AdrienSalle	0:32fd5a08c430	2258	}
AdrienSalle	0:32fd5a08c430	2259	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	2260	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2261	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2262	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2263	wait_us(100);
AdrienSalle	0:32fd5a08c430	2264	}
AdrienSalle	0:32fd5a08c430	2265	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2266	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	2267	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	2268	rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	2269	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	2270	}
AdrienSalle	0:32fd5a08c430	2271	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	2272	valueserial3=0;
AdrienSalle	0:32fd5a08c430	2273	}
AdrienSalle	0:32fd5a08c430	2274	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	2275	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2276	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2277	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2278	wait_us(100);
AdrienSalle	0:32fd5a08c430	2279	}
AdrienSalle	0:32fd5a08c430	2280	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2281	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	2282	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	2283	rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	2284	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	2285	}
AdrienSalle	0:32fd5a08c430	2286	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	2287	valueserial4=0;
AdrienSalle	0:32fd5a08c430	2288	}
AdrienSalle	0:32fd5a08c430	2289	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	2290	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2291	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2292	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2293	wait_us(100);
AdrienSalle	0:32fd5a08c430	2294	}
AdrienSalle	0:32fd5a08c430	2295	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2296	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	2297	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	2298	rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	2299	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	2300	}
AdrienSalle	0:32fd5a08c430	2301	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	2302	valueserial5=0;
AdrienSalle	0:32fd5a08c430	2303	}
AdrienSalle	0:32fd5a08c430	2304	}
AdrienSalle	0:32fd5a08c430	2305
AdrienSalle	0:32fd5a08c430	2306	/*for(uint8_t i = 0; i < 9 ; i++)
AdrienSalle	0:32fd5a08c430	2307	{
AdrienSalle	0:32fd5a08c430	2308	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2309	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2310	wait_us(100);
AdrienSalle	0:32fd5a08c430	2311	}
AdrienSalle	0:32fd5a08c430	2312
AdrienSalle	0:32fd5a08c430	2313	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2314	uint8_t rxBuf[12];
AdrienSalle	0:32fd5a08c430	2315	//wait_ms(3);
AdrienSalle	0:32fd5a08c430	2316	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2317	if(flag_serial1_receive)
AdrienSalle	0:32fd5a08c430	2318	{
AdrienSalle	0:32fd5a08c430	2319	for(unsigned char i4=0;i4<Size_trame_serial1; i4++)
AdrienSalle	0:32fd5a08c430	2320	{
AdrienSalle	0:32fd5a08c430	2321	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	2322	////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	2323	}
AdrienSalle	0:32fd5a08c430	2324	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2325	valueserial1=0;
AdrienSalle	0:32fd5a08c430	2326	}*/
AdrienSalle	0:32fd5a08c430	2327
AdrienSalle	0:32fd5a08c430	2328	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
AdrienSalle	0:32fd5a08c430	2329	if (chksum1 != rxBuf[5]) {
AdrienSalle	0:32fd5a08c430	2330	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	2331	pc->printf("Checksum1 fault\n");
AdrienSalle	0:32fd5a08c430	2332	#endif*/
AdrienSalle	0:32fd5a08c430	2333	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2334	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2335	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2336	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	2337	}
AdrienSalle	0:32fd5a08c430	2338	return -1;
AdrienSalle	0:32fd5a08c430	2339	}
AdrienSalle	0:32fd5a08c430	2340	// Checksum2
AdrienSalle	0:32fd5a08c430	2341	uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle	0:32fd5a08c430	2342	if (chksum2 != rxBuf[6]) {
AdrienSalle	0:32fd5a08c430	2343	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	2344	pc->printf("Checksum2 fault\n");
AdrienSalle	0:32fd5a08c430	2345	#endif*/
AdrienSalle	0:32fd5a08c430	2346	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2347	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2348	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2349	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	2350	}
AdrienSalle	0:32fd5a08c430	2351	return -1;
AdrienSalle	0:32fd5a08c430	2352	}
AdrienSalle	0:32fd5a08c430	2353	tensionMIN = rxBuf[9];
AdrienSalle	0:32fd5a08c430	2354	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	2355	pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle	0:32fd5a08c430	2356	#endif*/
AdrienSalle	0:32fd5a08c430	2357	//}
AdrienSalle	0:32fd5a08c430	2358	return tensionMIN;
AdrienSalle	0:32fd5a08c430	2359	}
AdrienSalle	0:32fd5a08c430	2360	//-------------fonction pour controle la tension min pour un servo--------------
AdrienSalle	0:32fd5a08c430	2361	void Set_Tension_MIN(int8_t id,uint8_t Tension_Min, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	2362	{
AdrienSalle	0:32fd5a08c430	2363	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	2364	uint8_t txBuf[10];
AdrienSalle	0:32fd5a08c430	2365
AdrienSalle	0:32fd5a08c430	2366	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2367	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2368	txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
AdrienSalle	0:32fd5a08c430	2369	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	2370	txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
AdrienSalle	0:32fd5a08c430	2371	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	2372	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	2373	txBuf[7] = RAM_MIN_VOLTAGE;
AdrienSalle	0:32fd5a08c430	2374	txBuf[8] = BYTE1; // Length
AdrienSalle	0:32fd5a08c430	2375	txBuf[9] = Tension_Min;
AdrienSalle	0:32fd5a08c430	2376	// Check Sum1 and Check Sum2
AdrienSalle	0:32fd5a08c430	2377	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
AdrienSalle	0:32fd5a08c430	2378	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	2379
AdrienSalle	0:32fd5a08c430	2380	//pc.printf(" tensionMIN ");
AdrienSalle	0:32fd5a08c430	2381	/*for(uint8_t i = 0; i < 10 ; i++)
AdrienSalle	0:32fd5a08c430	2382	{
AdrienSalle	0:32fd5a08c430	2383	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2384	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2385	wait_us(100);
AdrienSalle	0:32fd5a08c430	2386	}*/
AdrienSalle	0:32fd5a08c430	2387	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2388	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	2389	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2390	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2391	wait_us(100);
AdrienSalle	0:32fd5a08c430	2392	}
AdrienSalle	0:32fd5a08c430	2393	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2394	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	2395	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2396	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2397	wait_us(100);
AdrienSalle	0:32fd5a08c430	2398	}
AdrienSalle	0:32fd5a08c430	2399	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	2400	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	2401	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2402	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2403	wait_us(100);
AdrienSalle	0:32fd5a08c430	2404	}
AdrienSalle	0:32fd5a08c430	2405	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	2406	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	2407	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2408	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2409	wait_us(100);
AdrienSalle	0:32fd5a08c430	2410	}
AdrienSalle	0:32fd5a08c430	2411	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	2412	for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle	0:32fd5a08c430	2413	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2414	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2415	wait_us(100);
AdrienSalle	0:32fd5a08c430	2416	}
AdrienSalle	0:32fd5a08c430	2417	}
AdrienSalle	0:32fd5a08c430	2418	//wait_ms(3);
AdrienSalle	0:32fd5a08c430	2419	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2420	}
AdrienSalle	0:32fd5a08c430	2421	//------------fonction pour lire la tension d'un servo-------------
AdrienSalle	0:32fd5a08c430	2422	int8_t Get_Tension_actuelle(int8_t id, uint8_t numero_serial)
AdrienSalle	0:32fd5a08c430	2423	{
AdrienSalle	0:32fd5a08c430	2424	serial_numero = numero_serial;
AdrienSalle	0:32fd5a08c430	2425	uint8_t txBuf[9];
AdrienSalle	0:32fd5a08c430	2426	int8_t tension = 0;
AdrienSalle	0:32fd5a08c430	2427
AdrienSalle	0:32fd5a08c430	2428	txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2429	txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle	0:32fd5a08c430	2430	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle	0:32fd5a08c430	2431	txBuf[3] = id; // Servo ID
AdrienSalle	0:32fd5a08c430	2432	txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03)
AdrienSalle	0:32fd5a08c430	2433	txBuf[5] = 0; // Checksum1
AdrienSalle	0:32fd5a08c430	2434	txBuf[6] = 0; // Checksum2
AdrienSalle	0:32fd5a08c430	2435	txBuf[7] = RAM_VOLTAGE;
AdrienSalle	0:32fd5a08c430	2436	txBuf[8] = BYTE2; // Length
AdrienSalle	0:32fd5a08c430	2437	// Check Sum1 and Check Sum2
AdrienSalle	0:32fd5a08c430	2438	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle	0:32fd5a08c430	2439	txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle	0:32fd5a08c430	2440
AdrienSalle	0:32fd5a08c430	2441	//pc.printf(" tension ");
AdrienSalle	0:32fd5a08c430	2442
AdrienSalle	0:32fd5a08c430	2443	uint8_t rxBuf[13];
AdrienSalle	0:32fd5a08c430	2444
AdrienSalle	0:32fd5a08c430	2445	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2446	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2447	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2448	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2449	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2450	wait_us(100);
AdrienSalle	0:32fd5a08c430	2451	}
AdrienSalle	0:32fd5a08c430	2452
AdrienSalle	0:32fd5a08c430	2453	//send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2454	//uint8_t rxBuf[13];
AdrienSalle	0:32fd5a08c430	2455	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2456	if(flag_serial1_receive) {
AdrienSalle	0:32fd5a08c430	2457	for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle	0:32fd5a08c430	2458	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	2459	//////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	2460	}
AdrienSalle	0:32fd5a08c430	2461	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2462	valueserial1=0;
AdrienSalle	0:32fd5a08c430	2463	}
AdrienSalle	0:32fd5a08c430	2464	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2465	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2466	while(serial2.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2467	serial2.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2468	wait_us(100);
AdrienSalle	0:32fd5a08c430	2469	}
AdrienSalle	0:32fd5a08c430	2470	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2471	if(flag_serial2_receive) {
AdrienSalle	0:32fd5a08c430	2472	for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle	0:32fd5a08c430	2473	rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle	0:32fd5a08c430	2474	//////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle	0:32fd5a08c430	2475	}
AdrienSalle	0:32fd5a08c430	2476	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	2477	valueserial2=0;
AdrienSalle	0:32fd5a08c430	2478	}
AdrienSalle	0:32fd5a08c430	2479	} else if(numero_serial == 3) {
AdrienSalle	0:32fd5a08c430	2480	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2481	while(serial3.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2482	serial3.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2483	wait_us(100);
AdrienSalle	0:32fd5a08c430	2484	}
AdrienSalle	0:32fd5a08c430	2485	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2486	if(flag_serial3_receive) {
AdrienSalle	0:32fd5a08c430	2487	for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle	0:32fd5a08c430	2488	rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle	0:32fd5a08c430	2489	//////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle	0:32fd5a08c430	2490	}
AdrienSalle	0:32fd5a08c430	2491	flag_serial3_receive=0;
AdrienSalle	0:32fd5a08c430	2492	valueserial3=0;
AdrienSalle	0:32fd5a08c430	2493	}
AdrienSalle	0:32fd5a08c430	2494	} else if(numero_serial == 4) {
AdrienSalle	0:32fd5a08c430	2495	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2496	while(serial4.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2497	serial4.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2498	wait_us(100);
AdrienSalle	0:32fd5a08c430	2499	}
AdrienSalle	0:32fd5a08c430	2500	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2501	if(flag_serial4_receive) {
AdrienSalle	0:32fd5a08c430	2502	for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle	0:32fd5a08c430	2503	rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle	0:32fd5a08c430	2504	//////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle	0:32fd5a08c430	2505	}
AdrienSalle	0:32fd5a08c430	2506	flag_serial4_receive=0;
AdrienSalle	0:32fd5a08c430	2507	valueserial4=0;
AdrienSalle	0:32fd5a08c430	2508	}
AdrienSalle	0:32fd5a08c430	2509	} else if(numero_serial == 5) {
AdrienSalle	0:32fd5a08c430	2510	for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle	0:32fd5a08c430	2511	while(serial5.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2512	serial5.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2513	wait_us(100);
AdrienSalle	0:32fd5a08c430	2514	}
AdrienSalle	0:32fd5a08c430	2515	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2516	if(flag_serial5_receive) {
AdrienSalle	0:32fd5a08c430	2517	for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle	0:32fd5a08c430	2518	rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle	0:32fd5a08c430	2519	//////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle	0:32fd5a08c430	2520	}
AdrienSalle	0:32fd5a08c430	2521	flag_serial5_receive=0;
AdrienSalle	0:32fd5a08c430	2522	valueserial5=0;
AdrienSalle	0:32fd5a08c430	2523	}
AdrienSalle	0:32fd5a08c430	2524	}
AdrienSalle	0:32fd5a08c430	2525	/*
AdrienSalle	0:32fd5a08c430	2526	for(uint8_t i = 0; i < 9 ; i++)
AdrienSalle	0:32fd5a08c430	2527	{
AdrienSalle	0:32fd5a08c430	2528	while(serial1.writeable() == 0);
AdrienSalle	0:32fd5a08c430	2529	serial1.putc(txBuf[i]);
AdrienSalle	0:32fd5a08c430	2530	wait_us(100);
AdrienSalle	0:32fd5a08c430	2531	}
AdrienSalle	0:32fd5a08c430	2532
AdrienSalle	0:32fd5a08c430	2533	// send packet (mbed -> herkulex)
AdrienSalle	0:32fd5a08c430	2534	uint8_t rxBuf[13];
AdrienSalle	0:32fd5a08c430	2535	//wait_ms(3);
AdrienSalle	0:32fd5a08c430	2536	wait_ms(TEMPO_R);
AdrienSalle	0:32fd5a08c430	2537	if(flag_serial1_receive)
AdrienSalle	0:32fd5a08c430	2538	{
AdrienSalle	0:32fd5a08c430	2539	for(unsigned char i4=0;i4<Size_trame_serial1; i4++)
AdrienSalle	0:32fd5a08c430	2540	{
AdrienSalle	0:32fd5a08c430	2541	rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle	0:32fd5a08c430	2542	////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle	0:32fd5a08c430	2543	}
AdrienSalle	0:32fd5a08c430	2544	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2545	valueserial1=0;
AdrienSalle	0:32fd5a08c430	2546	}
AdrienSalle	0:32fd5a08c430	2547	*/
AdrienSalle	0:32fd5a08c430	2548
AdrienSalle	0:32fd5a08c430	2549	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
AdrienSalle	0:32fd5a08c430	2550	if (chksum1 != rxBuf[5]) {
AdrienSalle	0:32fd5a08c430	2551	/*#ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	2552	pc->printf("Checksum1 fault\n");
AdrienSalle	0:32fd5a08c430	2553	#endif*/
AdrienSalle	0:32fd5a08c430	2554	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2555	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2556	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2557	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	2558	}
AdrienSalle	0:32fd5a08c430	2559	return -1;
AdrienSalle	0:32fd5a08c430	2560	}
AdrienSalle	0:32fd5a08c430	2561	// Checksum2
AdrienSalle	0:32fd5a08c430	2562	uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle	0:32fd5a08c430	2563	if (chksum2 != rxBuf[6]) {
AdrienSalle	0:32fd5a08c430	2564	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	2565	pc->printf("Checksum2 fault\n");
AdrienSalle	0:32fd5a08c430	2566	#endif*/
AdrienSalle	0:32fd5a08c430	2567	if(numero_serial == 1) {
AdrienSalle	0:32fd5a08c430	2568	flag_serial1_receive=0;
AdrienSalle	0:32fd5a08c430	2569	} else if(numero_serial == 2) {
AdrienSalle	0:32fd5a08c430	2570	flag_serial2_receive=0;
AdrienSalle	0:32fd5a08c430	2571	}
AdrienSalle	0:32fd5a08c430	2572	return -1;
AdrienSalle	0:32fd5a08c430	2573	}
AdrienSalle	0:32fd5a08c430	2574	tension = rxBuf[9];
AdrienSalle	0:32fd5a08c430	2575	/* #ifdef HERKULEX_DEBUG
AdrienSalle	0:32fd5a08c430	2576	pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle	0:32fd5a08c430	2577	#endif*/
AdrienSalle	0:32fd5a08c430	2578	//}
AdrienSalle	0:32fd5a08c430	2579	return tension;
AdrienSalle	0:32fd5a08c430	2580	}
AdrienSalle	0:32fd5a08c430	2581
AdrienSalle	0:32fd5a08c430	2582
AdrienSalle	0:32fd5a08c430	2583	//-----------------------------------------------------------------------------------------
AdrienSalle	0:32fd5a08c430	2584	void servo_interrupt_en(void)
AdrienSalle	0:32fd5a08c430	2585	{
AdrienSalle	0:32fd5a08c430	2586	Interrupt1_en();
AdrienSalle	0:32fd5a08c430	2587	Interrupt2_en();
AdrienSalle	0:32fd5a08c430	2588	Interrupt3_en();
AdrienSalle	0:32fd5a08c430	2589	Interrupt4_en();
AdrienSalle	0:32fd5a08c430	2590	Interrupt5_en();
AdrienSalle	0:32fd5a08c430	2591	}
maximilienlv	16:fe49d838a0cc	2592
maximilienlv	16:fe49d838a0cc	2593	int8_t Get_Led(uint8_t id, uint8_t numero_serie)
maximilienlv	16:fe49d838a0cc	2594	{
maximilienlv	16:fe49d838a0cc	2595	char numero_serial = numero_serie;
maximilienlv	16:fe49d838a0cc	2596	uint8_t txBuf[7];
maximilienlv	16:fe49d838a0cc	2597	txBuf[0] = HEADER; // Packet Header (0xFF)
maximilienlv	16:fe49d838a0cc	2598	txBuf[1] = HEADER; // Packet Header (0xFF)
maximilienlv	16:fe49d838a0cc	2599	txBuf[2] = 0x07; // Packet Size
maximilienlv	16:fe49d838a0cc	2600	txBuf[3] = id; // Servo ID
maximilienlv	16:fe49d838a0cc	2601	txBuf[4] = 0x05; // Command Ram Write (0x03) *On choisi le CMD pour écrire dans un registre
maximilienlv	16:fe49d838a0cc	2602	txBuf[5] = 0; // Checksum1
maximilienlv	16:fe49d838a0cc	2603	txBuf[6] = 0; // Checksum2
maximilienlv	16:fe49d838a0cc	2604
maximilienlv	16:fe49d838a0cc	2605	// calcul des checksums //
maximilienlv	16:fe49d838a0cc	2606	txBuf[5]=(txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE; // checksum1
maximilienlv	16:fe49d838a0cc	2607	txBuf[5] &= 0xFE; // checksum1
maximilienlv	16:fe49d838a0cc	2608	txBuf[6] = (~txBuf[5] & 0xFE);//checksum2
maximilienlv	16:fe49d838a0cc	2609
maximilienlv	16:fe49d838a0cc	2610
maximilienlv	16:fe49d838a0cc	2611	// envoi //
maximilienlv	16:fe49d838a0cc	2612	//txPacket(0x07,txBuf,numero_serial);
maximilienlv	16:fe49d838a0cc	2613	//serial1.putc(0xAA);
maximilienlv	16:fe49d838a0cc	2614	//reception
maximilienlv	16:fe49d838a0cc	2615
maximilienlv	16:fe49d838a0cc	2616
maximilienlv	16:fe49d838a0cc	2617
maximilienlv	16:fe49d838a0cc	2618	uint8_t rxBuf[9];
maximilienlv	16:fe49d838a0cc	2619
maximilienlv	16:fe49d838a0cc	2620	if(numero_serial == 1) {
maximilienlv	16:fe49d838a0cc	2621	for(uint8_t i = 0; i < 7 ; i++) {
maximilienlv	16:fe49d838a0cc	2622	while(serial1.writeable() == 0);
maximilienlv	16:fe49d838a0cc	2623	serial1.putc(txBuf[i]);
maximilienlv	16:fe49d838a0cc	2624	wait_us(100);
maximilienlv	16:fe49d838a0cc	2625	}
maximilienlv	16:fe49d838a0cc	2626	wait_ms(TEMPO_R);
maximilienlv	16:fe49d838a0cc	2627	if(flag_serial1_receive) {
maximilienlv	16:fe49d838a0cc	2628	for(unsigned char i4=0; i4<8; i4++) {
maximilienlv	16:fe49d838a0cc	2629	rxBuf[i4] = char_receive_serial1[i4];
maximilienlv	16:fe49d838a0cc	2630	}
maximilienlv	16:fe49d838a0cc	2631	flag_serial1_receive=0;
maximilienlv	16:fe49d838a0cc	2632	valueserial1=0;
maximilienlv	16:fe49d838a0cc	2633	}
maximilienlv	16:fe49d838a0cc	2634	} else if(numero_serial == 2) {
maximilienlv	16:fe49d838a0cc	2635	for(uint8_t i = 0; i < 7 ; i++) {
maximilienlv	16:fe49d838a0cc	2636	while(serial2.writeable() == 0);
maximilienlv	16:fe49d838a0cc	2637	serial2.putc(txBuf[i]);
maximilienlv	16:fe49d838a0cc	2638	wait_us(100);
maximilienlv	16:fe49d838a0cc	2639	}
maximilienlv	16:fe49d838a0cc	2640	wait_ms(TEMPO_R);
maximilienlv	16:fe49d838a0cc	2641	if(flag_serial2_receive) {
maximilienlv	16:fe49d838a0cc	2642	for(unsigned char i4=0; i4<8; i4++) {
maximilienlv	16:fe49d838a0cc	2643	rxBuf[i4] = char_receive_serial2[i4];
maximilienlv	16:fe49d838a0cc	2644	}
maximilienlv	16:fe49d838a0cc	2645	flag_serial2_receive=0;
maximilienlv	16:fe49d838a0cc	2646	valueserial2=0;
maximilienlv	16:fe49d838a0cc	2647	}
maximilienlv	16:fe49d838a0cc	2648	} else if(numero_serial == 3) {
maximilienlv	16:fe49d838a0cc	2649	for(uint8_t i = 0; i < 7 ; i++) {
maximilienlv	16:fe49d838a0cc	2650	while(serial3.writeable() == 0);
maximilienlv	16:fe49d838a0cc	2651	serial3.putc(txBuf[i]);
maximilienlv	16:fe49d838a0cc	2652	wait_us(100);
maximilienlv	16:fe49d838a0cc	2653	}
maximilienlv	16:fe49d838a0cc	2654	wait_ms(TEMPO_R);
maximilienlv	16:fe49d838a0cc	2655	if(flag_serial3_receive) {
maximilienlv	16:fe49d838a0cc	2656	for(unsigned char i4=0; i4<8; i4++) {
maximilienlv	16:fe49d838a0cc	2657	rxBuf[i4] = char_receive_serial3[i4];
maximilienlv	16:fe49d838a0cc	2658	}
maximilienlv	16:fe49d838a0cc	2659	flag_serial3_receive=0;
maximilienlv	16:fe49d838a0cc	2660	valueserial3=0;
maximilienlv	16:fe49d838a0cc	2661	}
maximilienlv	16:fe49d838a0cc	2662	} else if(numero_serial == 4) {
maximilienlv	16:fe49d838a0cc	2663	for(uint8_t i = 0; i < 7 ; i++) {
maximilienlv	16:fe49d838a0cc	2664	while(serial4.writeable() == 0);
maximilienlv	16:fe49d838a0cc	2665	serial4.putc(txBuf[i]);
maximilienlv	16:fe49d838a0cc	2666	wait_us(100);
maximilienlv	16:fe49d838a0cc	2667	}
maximilienlv	16:fe49d838a0cc	2668	wait_ms(TEMPO_R);
maximilienlv	16:fe49d838a0cc	2669	if(flag_serial4_receive) {
maximilienlv	16:fe49d838a0cc	2670	for(unsigned char i4=0; i4<8; i4++) {
maximilienlv	16:fe49d838a0cc	2671	rxBuf[i4] = char_receive_serial4[i4];
maximilienlv	16:fe49d838a0cc	2672	}
maximilienlv	16:fe49d838a0cc	2673	flag_serial4_receive=0;
maximilienlv	16:fe49d838a0cc	2674	valueserial4=0;
maximilienlv	16:fe49d838a0cc	2675	}
maximilienlv	16:fe49d838a0cc	2676	} else if(numero_serial == 5) {
maximilienlv	16:fe49d838a0cc	2677	for(uint8_t i = 0; i < 7 ; i++) {
maximilienlv	16:fe49d838a0cc	2678	while(serial5.writeable() == 0);
maximilienlv	16:fe49d838a0cc	2679	serial5.putc(txBuf[i]);
maximilienlv	16:fe49d838a0cc	2680	wait_us(100);
maximilienlv	16:fe49d838a0cc	2681	}
maximilienlv	16:fe49d838a0cc	2682	wait_ms(TEMPO_R);
maximilienlv	16:fe49d838a0cc	2683	if(flag_serial5_receive) {
maximilienlv	16:fe49d838a0cc	2684	for(unsigned char i4=0; i4<8; i4++) {
maximilienlv	16:fe49d838a0cc	2685	rxBuf[i4] = char_receive_serial5[i4];
maximilienlv	16:fe49d838a0cc	2686	}
maximilienlv	16:fe49d838a0cc	2687	flag_serial5_receive=0;
maximilienlv	16:fe49d838a0cc	2688	valueserial5=0;
maximilienlv	16:fe49d838a0cc	2689	}
maximilienlv	16:fe49d838a0cc	2690	}
maximilienlv	16:fe49d838a0cc	2691
maximilienlv	16:fe49d838a0cc	2692	// Checksum1
maximilienlv	16:fe49d838a0cc	2693	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]) & 0xFE;
maximilienlv	16:fe49d838a0cc	2694	if (chksum1 != rxBuf[5]) {
maximilienlv	16:fe49d838a0cc	2695	if(numero_serial == 1) {
maximilienlv	16:fe49d838a0cc	2696	flag_serial1_receive=0;
maximilienlv	16:fe49d838a0cc	2697	} else if(numero_serial == 2) {
maximilienlv	16:fe49d838a0cc	2698	flag_serial2_receive=0;
maximilienlv	16:fe49d838a0cc	2699	} else if(numero_serial == 3) {
maximilienlv	16:fe49d838a0cc	2700	flag_serial3_receive=0;
maximilienlv	16:fe49d838a0cc	2701	}
maximilienlv	16:fe49d838a0cc	2702	return -1;
maximilienlv	16:fe49d838a0cc	2703	}
maximilienlv	16:fe49d838a0cc	2704	// Checksum2
maximilienlv	16:fe49d838a0cc	2705	uint8_t chksum2 = (~rxBuf[5]&0xFE);
maximilienlv	16:fe49d838a0cc	2706	if (chksum2 != rxBuf[6]) {
maximilienlv	16:fe49d838a0cc	2707	if(numero_serial == 1) {
maximilienlv	16:fe49d838a0cc	2708	flag_serial1_receive=0;
maximilienlv	16:fe49d838a0cc	2709	} else if(numero_serial == 2) {
maximilienlv	16:fe49d838a0cc	2710	flag_serial2_receive=0;
maximilienlv	16:fe49d838a0cc	2711	} else if(numero_serial == 3) {
maximilienlv	16:fe49d838a0cc	2712	flag_serial3_receive=0;
maximilienlv	16:fe49d838a0cc	2713	}
maximilienlv	16:fe49d838a0cc	2714	return -1;
maximilienlv	16:fe49d838a0cc	2715	}
maximilienlv	16:fe49d838a0cc	2716	int8_t couleur_hrk = rxBuf[7]; // Couleur
maximilienlv	16:fe49d838a0cc	2717
maximilienlv	16:fe49d838a0cc	2718	return couleur_hrk;
maximilienlv	16:fe49d838a0cc	2719	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	11 Nov 2021
Imports:	0
Forks:	0
Commits:	20
Dependents:	0
Dependencies:	0
Followers:	1

fonctions_herkulex.cpp@16:fe49d838a0cc, 2020-07-09 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning