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@11:542de77801ba, 2019-05-25 (annotated)

Committer:: Artiom
Date:: Sat May 25 12:06:02 2019 +0000
Revision:: 11:542de77801ba
Parent:: 9:0f09e3ee52e9
Child:: 12:562aa318952c

maj fonction verification en booleen

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

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@11:542de77801ba, 2019-05-25 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning