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@19:aaa97ea138fd, 2021-07-12 (annotated)

Committer:: thesnowfoxy
Date:: Mon Jul 12 13:10:29 2021 +0000
Revision:: 19:aaa97ea138fd
Parent:: 18:41bce61db461

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

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@19:aaa97ea138fd, 2021-07-12 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning