CAN_Servomoteur_V1_Haoxuan - CAN_Servomoteur_V1_Haoxuan avec lib de servo

CRAC Team » Code » CAN_Servomoteur_V1_Haoxuan

CRAC Team / Mbed 2 deprecated CAN_Servomoteur_V1_Haoxuan

CAN_Servomoteur_V1_Haoxuan avec lib de servo

Dependencies: mbed

lib.cpp@0:3b683da943e6, 2018-04-12 (annotated)

Committer:: Shanglin
Date:: Thu Apr 12 08:54:49 2018 +0000
Revision:: 0:3b683da943e6

CAN_Servo_V1_Haoxuan_avec_lib_servo

Who changed what in which revision?

User	Revision	Line number	New contents of line
Shanglin	0:3b683da943e6	1	#include "mbed.h"
Shanglin	0:3b683da943e6	2	#include "lib.h"
Shanglin	0:3b683da943e6	3
Shanglin	0:3b683da943e6	4	//RawSerial pc(SERIAL_TX, SERIAL_RX, 57600);
Shanglin	0:3b683da943e6	5	//RawSerial serial4(PB_6,PB_7,57600);
Shanglin	0:3b683da943e6	6
Shanglin	0:3b683da943e6	7
Shanglin	0:3b683da943e6	8	#define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
Shanglin	0:3b683da943e6	9	#define tolerance_en_position_negatif -16
Shanglin	0:3b683da943e6	10	#define V_b 45
Shanglin	0:3b683da943e6	11	#define V_m 45
Shanglin	0:3b683da943e6	12	#define V_h 45
Shanglin	0:3b683da943e6	13	#define TEMPO_R 30
Shanglin	0:3b683da943e6	14
Shanglin	0:3b683da943e6	15	//------------------------------------------a choisir---------------------------
Shanglin	0:3b683da943e6	16	#define nombre_servomoteur 3 //
Shanglin	0:3b683da943e6	17	//
Shanglin	0:3b683da943e6	18	#define ID_1 0xFD //=253 premiere //
Shanglin	0:3b683da943e6	19	#define ID_3 0x0D //=13 troixieme //
Shanglin	0:3b683da943e6	20	#define ID_2 0x0C //=12 deuxieme //
Shanglin	0:3b683da943e6	21	#define ID_4 0x09 //Avec engrenage
Shanglin	0:3b683da943e6	22	#define Pompe_pin1 PA_15 //Pin de Pompe
Shanglin	0:3b683da943e6	23	//------------------------------------------------------------------------------
Shanglin	0:3b683da943e6	24
Shanglin	0:3b683da943e6	25	uint16_t Pos_Engrenage_centre = 351;
Shanglin	0:3b683da943e6	26	uint16_t Pos_Engrenage_gauche = 607;
Shanglin	0:3b683da943e6	27	uint16_t Pos_Engrenage_droit = 108;
Shanglin	0:3b683da943e6	28	uint16_t Pos_Engrenage_Vers_Droit = 62;
Shanglin	0:3b683da943e6	29	uint16_t Pos_Engrenage_Vers_Gauche = 630;
Shanglin	0:3b683da943e6	30
Shanglin	0:3b683da943e6	31	//----------------------------variables de reception----------------------------
Shanglin	0:3b683da943e6	32	uint8_t rx[300];
Shanglin	0:3b683da943e6	33	uint8_t rx2[256];
Shanglin	0:3b683da943e6	34	unsigned char size_reponse=100;
Shanglin	0:3b683da943e6	35	unsigned char recevoir = 0;
Shanglin	0:3b683da943e6	36	unsigned char i2 = 0;
Shanglin	0:3b683da943e6	37	unsigned char flag_serial4_receive2 = 0;
Shanglin	0:3b683da943e6	38	//--------------------variables et fonction de verification---------------------
Shanglin	0:3b683da943e6	39	int16_t pos_position = 0, get_pos = 0, pos_ID = 0;
Shanglin	0:3b683da943e6	40	uint8_t pos_led = 0, Status = 0,iID = 0;
Shanglin	0:3b683da943e6	41	uint8_t nombre_servo = 0;
Shanglin	0:3b683da943e6	42	uint8_t pos_time = 0;
Shanglin	0:3b683da943e6	43	uint16_t position_servo_mul[20];
Shanglin	0:3b683da943e6	44	uint8_t data_servo_mul[40];
Shanglin	0:3b683da943e6	45	uint8_t flag_correction = 0;
Shanglin	0:3b683da943e6	46	float new_tempo=0;
Shanglin	0:3b683da943e6	47	float tab_tempo[20];
Shanglin	0:3b683da943e6	48	uint16_t position_servo_mul_different[20];
Shanglin	0:3b683da943e6	49	uint8_t data_servo_mul_different[60];
Shanglin	0:3b683da943e6	50	int8_t my_Tor = 0;
Shanglin	0:3b683da943e6	51	int8_t Tension_inter = 0;
Shanglin	0:3b683da943e6	52	float Tension = 0;
Shanglin	0:3b683da943e6	53	uint8_t coeffient_time = 1;
Shanglin	0:3b683da943e6	54	uint8_t veri = 0;
Shanglin	0:3b683da943e6	55	typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ;
Shanglin	0:3b683da943e6	56	static type_etat etat=pos;
Shanglin	0:3b683da943e6	57	void verifacation()
Shanglin	0:3b683da943e6	58	{
Shanglin	0:3b683da943e6	59	uint8_t i = 0;
Shanglin	0:3b683da943e6	60	switch(etat) {
Shanglin	0:3b683da943e6	61	case pos:
Shanglin	0:3b683da943e6	62	//------------------------Status--------------------
Shanglin	0:3b683da943e6	63	Status = getStatus(pos_ID);
Shanglin	0:3b683da943e6	64	wait_ms(3);
Shanglin	0:3b683da943e6	65	pc.printf("status = %d",Status);
Shanglin	0:3b683da943e6	66	switch(Status) {
Shanglin	0:3b683da943e6	67	case 0:
Shanglin	0:3b683da943e6	68	break;
Shanglin	0:3b683da943e6	69
Shanglin	0:3b683da943e6	70	case 2: //Exceed allowed POT limit
Shanglin	0:3b683da943e6	71	pc.printf("ERR-Depasse la limite de position\n");
Shanglin	0:3b683da943e6	72	//clean_ERR(pos_ID);
Shanglin	0:3b683da943e6	73	//wait_ms(500);
Shanglin	0:3b683da943e6	74	clear(pos_ID);
Shanglin	0:3b683da943e6	75	//positionControl(pos_ID, 1000, 3, GLED_ON);
Shanglin	0:3b683da943e6	76	wait_ms(3);
Shanglin	0:3b683da943e6	77	Status = getStatus(pos_ID);
Shanglin	0:3b683da943e6	78	wait_ms(3);
Shanglin	0:3b683da943e6	79	pc.printf("status = %d",Status);
Shanglin	0:3b683da943e6	80	break;
Shanglin	0:3b683da943e6	81	}
Shanglin	0:3b683da943e6	82	//------------------Torque et position------------------------------
Shanglin	0:3b683da943e6	83	my_Tor = Get_Torque(pos_ID);
Shanglin	0:3b683da943e6	84	wait_ms(5);
Shanglin	0:3b683da943e6	85	//pc.printf("my_Tor = %x\n",my_Tor);
Shanglin	0:3b683da943e6	86	while(my_Tor != 0x60) {
Shanglin	0:3b683da943e6	87	setTorque(pos_ID,TORQUE_ON);
Shanglin	0:3b683da943e6	88	my_Tor = Get_Torque(pos_ID);
Shanglin	0:3b683da943e6	89	wait_ms(5);
Shanglin	0:3b683da943e6	90	}
Shanglin	0:3b683da943e6	91	Tension_inter = Get_Tension_actuelle(pos_ID);
Shanglin	0:3b683da943e6	92	Tension = Tension_inter*0.074;
Shanglin	0:3b683da943e6	93	if(Tension <=6.60) {
Shanglin	0:3b683da943e6	94	coeffient_time = 6;
Shanglin	0:3b683da943e6	95	} else if(Tension <= 6.90) {
Shanglin	0:3b683da943e6	96	coeffient_time = 4;
Shanglin	0:3b683da943e6	97	} else if(Tension <= 7.10) {
Shanglin	0:3b683da943e6	98	coeffient_time = 2;
Shanglin	0:3b683da943e6	99	} else if(Tension > 7.10) {
Shanglin	0:3b683da943e6	100	coeffient_time = 1;
Shanglin	0:3b683da943e6	101	}
Shanglin	0:3b683da943e6	102	get_pos = getPos(pos_ID);
Shanglin	0:3b683da943e6	103	pc.printf("P4=%d ",get_pos);
Shanglin	0:3b683da943e6	104	if(((get_pos - pos_position)>tolerance_en_position)\|\|((get_pos - pos_position)<tolerance_en_position_negatif)) {
Shanglin	0:3b683da943e6	105	if((get_pos - pos_position)>tolerance_en_position) {
Shanglin	0:3b683da943e6	106	new_tempo=(get_pos - pos_position)0.084coeffient_time + 1;
Shanglin	0:3b683da943e6	107	if (new_tempo > 254) new_tempo = 254;
Shanglin	0:3b683da943e6	108	} else if((get_pos - pos_position)<tolerance_en_position_negatif) {
Shanglin	0:3b683da943e6	109	new_tempo=(get_pos - pos_position)0.084coeffient_time +1;
Shanglin	0:3b683da943e6	110	if (new_tempo > 254) new_tempo = 254;
Shanglin	0:3b683da943e6	111	}
Shanglin	0:3b683da943e6	112	positionControl(pos_ID, pos_position, new_tempo, pos_led);
Shanglin	0:3b683da943e6	113	pc.printf("Correction!\n");
Shanglin	0:3b683da943e6	114	}
Shanglin	0:3b683da943e6	115	break;
Shanglin	0:3b683da943e6	116	case pos_mul_complex:
Shanglin	0:3b683da943e6	117	//---------------------------Status---------------------------
Shanglin	0:3b683da943e6	118	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	119	Status = getStatus(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	120	pc.printf("status = %d",Status);
Shanglin	0:3b683da943e6	121	switch(Status) {
Shanglin	0:3b683da943e6	122	case 0:
Shanglin	0:3b683da943e6	123	break;
Shanglin	0:3b683da943e6	124
Shanglin	0:3b683da943e6	125	case 2: //Exceed allowed POT limit
Shanglin	0:3b683da943e6	126	//pc.printf("ERR-Depasse la limite de position\n");
Shanglin	0:3b683da943e6	127	//clean_ERR(id);
Shanglin	0:3b683da943e6	128	//wait_ms(500);
Shanglin	0:3b683da943e6	129	clear(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	130	//positionControl(id, 1000, 3, GLED_ON);
Shanglin	0:3b683da943e6	131	//wait_ms(3);
Shanglin	0:3b683da943e6	132	//Status = getStatus(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	133	//wait_ms(3);
Shanglin	0:3b683da943e6	134	//pc.printf("status = %d",Status);
Shanglin	0:3b683da943e6	135	break;
Shanglin	0:3b683da943e6	136	}
Shanglin	0:3b683da943e6	137	}
Shanglin	0:3b683da943e6	138	//----------------------Torque et position--------------------------
Shanglin	0:3b683da943e6	139	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	140	my_Tor = Get_Torque(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	141	while(my_Tor != 0x60) {
Shanglin	0:3b683da943e6	142	setTorque(data_servo_mul[1+2*i],TORQUE_ON);
Shanglin	0:3b683da943e6	143	my_Tor = Get_Torque(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	144	//pc.printf(" SET_TORQUE ");
Shanglin	0:3b683da943e6	145
Shanglin	0:3b683da943e6	146	Status = getStatus(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	147	clear(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	148	Status = getStatus(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	149	}
Shanglin	0:3b683da943e6	150	}
Shanglin	0:3b683da943e6	151	veri = 0;
Shanglin	0:3b683da943e6	152	while(veri < nombre_servo){
Shanglin	0:3b683da943e6	153	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	154	my_Tor = Get_Torque(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	155	while(my_Tor != 0x60) {
Shanglin	0:3b683da943e6	156	setTorque(data_servo_mul[1+2*i],TORQUE_ON);
Shanglin	0:3b683da943e6	157	my_Tor = Get_Torque(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	158	//pc.printf(" SET_TORQUE ");
Shanglin	0:3b683da943e6	159
Shanglin	0:3b683da943e6	160	Status = getStatus(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	161	clear(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	162	Status = getStatus(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	163	}
Shanglin	0:3b683da943e6	164	}
Shanglin	0:3b683da943e6	165	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	166	Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	167	Tension = Tension_inter*0.074;
Shanglin	0:3b683da943e6	168	if(Tension <=6.60) {
Shanglin	0:3b683da943e6	169	coeffient_time = 6;
Shanglin	0:3b683da943e6	170	} else if(Tension <= 6.90) {
Shanglin	0:3b683da943e6	171	coeffient_time = 4;
Shanglin	0:3b683da943e6	172	} else if(Tension <= 7.10) {
Shanglin	0:3b683da943e6	173	coeffient_time = 2;
Shanglin	0:3b683da943e6	174	} else if(Tension > 7.10) {
Shanglin	0:3b683da943e6	175	coeffient_time = 1;
Shanglin	0:3b683da943e6	176	}
Shanglin	0:3b683da943e6	177	get_pos = getPos(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	178	pc.printf("PosiM=%d ",get_pos);
Shanglin	0:3b683da943e6	179	if((get_pos - position_servo_mul[i])>tolerance_en_position) {
Shanglin	0:3b683da943e6	180	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
Shanglin	0:3b683da943e6	181	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Shanglin	0:3b683da943e6	182	flag_correction = 1;
Shanglin	0:3b683da943e6	183	} else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
Shanglin	0:3b683da943e6	184	tab_tempo[i]=(position_servo_mul[i] - get_pos)0.084coeffient_time+1;
Shanglin	0:3b683da943e6	185	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Shanglin	0:3b683da943e6	186	flag_correction = 1;
Shanglin	0:3b683da943e6	187	}
Shanglin	0:3b683da943e6	188	}
Shanglin	0:3b683da943e6	189	if(flag_correction == 1) {
Shanglin	0:3b683da943e6	190	new_tempo = 0;
Shanglin	0:3b683da943e6	191	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	192	if(tab_tempo[i]>new_tempo) {
Shanglin	0:3b683da943e6	193	new_tempo = tab_tempo[i];
Shanglin	0:3b683da943e6	194	}
Shanglin	0:3b683da943e6	195	}
Shanglin	0:3b683da943e6	196	flag_correction = 0;
Shanglin	0:3b683da943e6	197	positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul);
Shanglin	0:3b683da943e6	198	pc.printf("Correction!\n");
Shanglin	0:3b683da943e6	199	}
Shanglin	0:3b683da943e6	200	veri = 0;
Shanglin	0:3b683da943e6	201	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	202	get_pos = getPos(data_servo_mul[1+2*i]);
Shanglin	0:3b683da943e6	203	pc.printf("PosiM=%d ",get_pos);
Shanglin	0:3b683da943e6	204	if((get_pos - position_servo_mul[i])>tolerance_en_position) {
Shanglin	0:3b683da943e6	205	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
Shanglin	0:3b683da943e6	206	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Shanglin	0:3b683da943e6	207	flag_correction = 1;
Shanglin	0:3b683da943e6	208	} else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
Shanglin	0:3b683da943e6	209	tab_tempo[i]=(position_servo_mul[i] - get_pos)0.084coeffient_time+1;
Shanglin	0:3b683da943e6	210	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Shanglin	0:3b683da943e6	211	flag_correction = 1;
Shanglin	0:3b683da943e6	212	}else //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif))
Shanglin	0:3b683da943e6	213	{
Shanglin	0:3b683da943e6	214	veri++;
Shanglin	0:3b683da943e6	215	}
Shanglin	0:3b683da943e6	216	}
Shanglin	0:3b683da943e6	217	}
Shanglin	0:3b683da943e6	218	break;
Shanglin	0:3b683da943e6	219	case pos_mul_complex_different:
Shanglin	0:3b683da943e6	220	//---------------------------Status---------------------------
Shanglin	0:3b683da943e6	221	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	222	Status = getStatus(data_servo_mul_different[1+3*i]);
Shanglin	0:3b683da943e6	223	//pc.printf("status = %d",Status);
Shanglin	0:3b683da943e6	224	switch(Status) {
Shanglin	0:3b683da943e6	225	case 0:
Shanglin	0:3b683da943e6	226	break;
Shanglin	0:3b683da943e6	227
Shanglin	0:3b683da943e6	228	case 2: //Exceed allowed POT limit
Shanglin	0:3b683da943e6	229	//pc.printf("ERR-Depasse la limite de position\n");
Shanglin	0:3b683da943e6	230	//clean_ERR(id);
Shanglin	0:3b683da943e6	231	//wait_ms(500);
Shanglin	0:3b683da943e6	232	clear(data_servo_mul_different[1+3*i]);
Shanglin	0:3b683da943e6	233	//positionControl(id, 1000, 3, GLED_ON);
Shanglin	0:3b683da943e6	234	//wait_ms(3);
Shanglin	0:3b683da943e6	235	//Status = getStatus(data_servo_mul_different[1+2*i]);
Shanglin	0:3b683da943e6	236	//wait_ms(3);
Shanglin	0:3b683da943e6	237	//pc.printf("status = %d",Status);
Shanglin	0:3b683da943e6	238	break;
Shanglin	0:3b683da943e6	239	}
Shanglin	0:3b683da943e6	240	}
Shanglin	0:3b683da943e6	241	//-------------------Torque et position-----------------------------
Shanglin	0:3b683da943e6	242	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	243	my_Tor = Get_Torque(data_servo_mul_different[1+3*i]);
Shanglin	0:3b683da943e6	244	while(my_Tor != 0x60) {
Shanglin	0:3b683da943e6	245	setTorque(data_servo_mul_different[1+3*i],TORQUE_ON);
Shanglin	0:3b683da943e6	246	my_Tor = Get_Torque(data_servo_mul_different[1+3*i]);
Shanglin	0:3b683da943e6	247	//wait_ms(5);
Shanglin	0:3b683da943e6	248	//pc.printf(" SET_TORQUE ");
Shanglin	0:3b683da943e6	249	}
Shanglin	0:3b683da943e6	250	}
Shanglin	0:3b683da943e6	251	for(i=0; i<nombre_servo; i++) {
Shanglin	0:3b683da943e6	252	Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i]);
Shanglin	0:3b683da943e6	253	Tension = Tension_inter*0.074;
Shanglin	0:3b683da943e6	254	if(Tension <=6.60) {
Shanglin	0:3b683da943e6	255	coeffient_time = 6;
Shanglin	0:3b683da943e6	256	} else if(Tension <= 6.90) {
Shanglin	0:3b683da943e6	257	coeffient_time = 4;
Shanglin	0:3b683da943e6	258	} else if(Tension <= 7.10) {
Shanglin	0:3b683da943e6	259	coeffient_time = 2;
Shanglin	0:3b683da943e6	260	} else if(Tension > 7.10) {
Shanglin	0:3b683da943e6	261	coeffient_time = 1;
Shanglin	0:3b683da943e6	262	}
Shanglin	0:3b683da943e6	263	get_pos = getPos(data_servo_mul_different[1+3*i]);
Shanglin	0:3b683da943e6	264	pc.printf("PosiM=%d ",get_pos);
Shanglin	0:3b683da943e6	265	if((get_pos - position_servo_mul_different[i])>tolerance_en_position) {
Shanglin	0:3b683da943e6	266	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
Shanglin	0:3b683da943e6	267	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Shanglin	0:3b683da943e6	268	data_servo_mul_different[2+3*i] = tab_tempo[i];
Shanglin	0:3b683da943e6	269	flag_correction = 1;
Shanglin	0:3b683da943e6	270	} else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) {
Shanglin	0:3b683da943e6	271	tab_tempo[i]=(position_servo_mul_different[i] - get_pos)0.084coeffient_time+1;
Shanglin	0:3b683da943e6	272	if (tab_tempo[i] > 254) tab_tempo[i] = 254;
Shanglin	0:3b683da943e6	273	data_servo_mul_different[2+3*i] = tab_tempo[i];
Shanglin	0:3b683da943e6	274	flag_correction = 1;
Shanglin	0:3b683da943e6	275	}
Shanglin	0:3b683da943e6	276	}
Shanglin	0:3b683da943e6	277	if(flag_correction == 1) {
Shanglin	0:3b683da943e6	278	flag_correction = 0;
Shanglin	0:3b683da943e6	279	positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different);
Shanglin	0:3b683da943e6	280	pc.printf("Correction!\n");
Shanglin	0:3b683da943e6	281	}
Shanglin	0:3b683da943e6	282	break;
Shanglin	0:3b683da943e6	283	}
Shanglin	0:3b683da943e6	284	}
Shanglin	0:3b683da943e6	285	//---------------------fonction d'interruption de reception---------------------
Shanglin	0:3b683da943e6	286	unsigned char flag_perdu_info = 0, indicateur = 0, Size_trame = 0, old_valueserial4 = 0;
Shanglin	0:3b683da943e6	287	unsigned char char_receive_pc[100];
Shanglin	0:3b683da943e6	288	unsigned char char_receive_serial4[100];
Shanglin	0:3b683da943e6	289	unsigned char valueserial4=0;
Shanglin	0:3b683da943e6	290	unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial4_receive=0;
Shanglin	0:3b683da943e6	291	void receive_serial4()
Shanglin	0:3b683da943e6	292	{
Shanglin	0:3b683da943e6	293	char_receive_serial4[valueserial4]=serial4.getc();
Shanglin	0:3b683da943e6	294	automate();
Shanglin	0:3b683da943e6	295	}
Shanglin	0:3b683da943e6	296
Shanglin	0:3b683da943e6	297	void Interrupt4_en(void){
Shanglin	0:3b683da943e6	298	serial4.attach(&receive_serial4,Serial::RxIrq);
Shanglin	0:3b683da943e6	299	}
Shanglin	0:3b683da943e6	300
Shanglin	0:3b683da943e6	301	void automate()
Shanglin	0:3b683da943e6	302	{
Shanglin	0:3b683da943e6	303	typedef enum {Attente,FF,Size,Data} type_etat;
Shanglin	0:3b683da943e6	304	static type_etat etat=Attente;
Shanglin	0:3b683da943e6	305	//pc.printf("coucou");
Shanglin	0:3b683da943e6	306
Shanglin	0:3b683da943e6	307	//pc.printf("%d\r\n", char_receive_serial4[valueserial4]);
Shanglin	0:3b683da943e6	308
Shanglin	0:3b683da943e6	309	switch (etat) {
Shanglin	0:3b683da943e6	310	case Attente:
Shanglin	0:3b683da943e6	311	if(char_receive_serial4[0] == 0xFF)
Shanglin	0:3b683da943e6	312	{
Shanglin	0:3b683da943e6	313	etat = FF;
Shanglin	0:3b683da943e6	314	valueserial4 = 1;
Shanglin	0:3b683da943e6	315	}
Shanglin	0:3b683da943e6	316	break;
Shanglin	0:3b683da943e6	317	case FF:
Shanglin	0:3b683da943e6	318	if(char_receive_serial4[1] == 0xFF)
Shanglin	0:3b683da943e6	319	{
Shanglin	0:3b683da943e6	320	etat = Size;
Shanglin	0:3b683da943e6	321	valueserial4 = 2;
Shanglin	0:3b683da943e6	322	}
Shanglin	0:3b683da943e6	323	else
Shanglin	0:3b683da943e6	324	{
Shanglin	0:3b683da943e6	325	etat = Attente;
Shanglin	0:3b683da943e6	326	valueserial4 = 0;
Shanglin	0:3b683da943e6	327	flag_perdu_info = 1; //flag_perdu_info
Shanglin	0:3b683da943e6	328	}
Shanglin	0:3b683da943e6	329	break;
Shanglin	0:3b683da943e6	330	case Size:
Shanglin	0:3b683da943e6	331	if(char_receive_serial4[2] < 7)
Shanglin	0:3b683da943e6	332	{
Shanglin	0:3b683da943e6	333	etat = Attente;
Shanglin	0:3b683da943e6	334	valueserial4 = 0;
Shanglin	0:3b683da943e6	335	flag_perdu_info = 1; //flag_perdu_info
Shanglin	0:3b683da943e6	336	} else {
Shanglin	0:3b683da943e6	337	etat = Data;
Shanglin	0:3b683da943e6	338	old_valueserial4 = 2;
Shanglin	0:3b683da943e6	339	valueserial4 = 3;
Shanglin	0:3b683da943e6	340	}
Shanglin	0:3b683da943e6	341	Size_trame = char_receive_serial4[2];
Shanglin	0:3b683da943e6	342	break;
Shanglin	0:3b683da943e6	343
Shanglin	0:3b683da943e6	344	case Data:
Shanglin	0:3b683da943e6	345	if((valueserial4-2)==(Size_trame-3))
Shanglin	0:3b683da943e6	346	{
Shanglin	0:3b683da943e6	347	flag_serial4_receive = 1;
Shanglin	0:3b683da943e6	348	etat = Attente;
Shanglin	0:3b683da943e6	349	valueserial4 = 0;
Shanglin	0:3b683da943e6	350	} else {
Shanglin	0:3b683da943e6	351	valueserial4++;
Shanglin	0:3b683da943e6	352	}
Shanglin	0:3b683da943e6	353	break;
Shanglin	0:3b683da943e6	354
Shanglin	0:3b683da943e6	355	default:break;
Shanglin	0:3b683da943e6	356	}
Shanglin	0:3b683da943e6	357	}
Shanglin	0:3b683da943e6	358
Shanglin	0:3b683da943e6	359	//----------------xxxxx----fonction de fermture de serial-----------------------
Shanglin	0:3b683da943e6	360	/*void N_Herkulex()
Shanglin	0:3b683da943e6	361	{
Shanglin	0:3b683da943e6	362
Shanglin	0:3b683da943e6	363	if(Sv != NULL)
Shanglin	0:3b683da943e6	364	delete Sv;
Shanglin	0:3b683da943e6	365	if(recevoir==2) {
Shanglin	0:3b683da943e6	366	size_reponse = rx2[recevoir];
Shanglin	0:3b683da943e6	367	}
Shanglin	0:3b683da943e6	368	}*/
Shanglin	0:3b683da943e6	369	//--------------2e fonction d'effacer les erreurs (pas utilisee)----------------
Shanglin	0:3b683da943e6	370	int8_t clean_ERR(uint8_t id)
Shanglin	0:3b683da943e6	371	{
Shanglin	0:3b683da943e6	372	uint8_t Cx[11]; //{0xFF,0xFF,0x0B,0xFD,0x03,0xC6,0x38,0x30,0x02,0x00,0x00};
Shanglin	0:3b683da943e6	373	uint8_t Cr[9]; //{0xFF,0xFF,0x09,0xFD,0x04,0xC2,0x3C,0x30,0x02};
Shanglin	0:3b683da943e6	374
Shanglin	0:3b683da943e6	375	Cx[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	376	Cx[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	377	Cx[2] = MIN_PACKET_SIZE + 4; // Packet Size
Shanglin	0:3b683da943e6	378	Cx[3] = id; // Servo ID
Shanglin	0:3b683da943e6	379	Cx[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
Shanglin	0:3b683da943e6	380	Cx[5] = 0xC6; // Checksum1
Shanglin	0:3b683da943e6	381	Cx[6] = 0x38; // Checksum2
Shanglin	0:3b683da943e6	382	Cx[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address
Shanglin	0:3b683da943e6	383	Cx[8] = BYTE2; // Length
Shanglin	0:3b683da943e6	384	Cx[9] = 0;
Shanglin	0:3b683da943e6	385	Cx[10]= 0;
Shanglin	0:3b683da943e6	386
Shanglin	0:3b683da943e6	387	Cr[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	388	Cr[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	389	Cr[2] = MIN_PACKET_SIZE + 2; // Packet Size
Shanglin	0:3b683da943e6	390	Cr[3] = id; // Servo ID
Shanglin	0:3b683da943e6	391	Cr[4] = CMD_RAM_READ; // Command Ram Read
Shanglin	0:3b683da943e6	392	Cr[5] = 0xC2; // Checksum1
Shanglin	0:3b683da943e6	393	Cr[6] = 0x3C; // Checksum2
Shanglin	0:3b683da943e6	394	Cr[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address
Shanglin	0:3b683da943e6	395	Cr[8] = BYTE2; // Length
Shanglin	0:3b683da943e6	396
Shanglin	0:3b683da943e6	397	size_reponse = 13;
Shanglin	0:3b683da943e6	398	for(uint8_t i = 0; i < 11 ; i++) {
Shanglin	0:3b683da943e6	399	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	400	serial4.putc(Cx[i]);
Shanglin	0:3b683da943e6	401	}
Shanglin	0:3b683da943e6	402	wait_ms(0.3);
Shanglin	0:3b683da943e6	403	for(uint8_t i = 0; i < 9 ; i++) {
Shanglin	0:3b683da943e6	404	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	405	serial4.putc(Cr[i]);
Shanglin	0:3b683da943e6	406	}
Shanglin	0:3b683da943e6	407	wait_ms(0.5);
Shanglin	0:3b683da943e6	408
Shanglin	0:3b683da943e6	409	uint8_t rxBuf[13];
Shanglin	0:3b683da943e6	410	if(flag_serial4_receive2) {
Shanglin	0:3b683da943e6	411	for(i2 = 0; i2<13; i2++) {
Shanglin	0:3b683da943e6	412	rxBuf[i2] = rx2[i2];
Shanglin	0:3b683da943e6	413	pc.printf(" %x",rx2[i2]);
Shanglin	0:3b683da943e6	414	}
Shanglin	0:3b683da943e6	415	flag_serial4_receive2=0;
Shanglin	0:3b683da943e6	416	}
Shanglin	0:3b683da943e6	417	// Checksum1
Shanglin	0:3b683da943e6	418	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
Shanglin	0:3b683da943e6	419	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	420	return -1;
Shanglin	0:3b683da943e6	421	}
Shanglin	0:3b683da943e6	422	// Checksum2
Shanglin	0:3b683da943e6	423	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	424	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	425	return -1;
Shanglin	0:3b683da943e6	426	}
Shanglin	0:3b683da943e6	427	return 0;
Shanglin	0:3b683da943e6	428	}
Shanglin	0:3b683da943e6	429	//-------------------------fonction de transmission-----------------------------
Shanglin	0:3b683da943e6	430	//comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise
Shanglin	0:3b683da943e6	431	void txPacket(uint8_t packetSize, uint8_t* data)
Shanglin	0:3b683da943e6	432	{
Shanglin	0:3b683da943e6	433	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	434	pc->printf("[TX]");
Shanglin	0:3b683da943e6	435	#endif
Shanglin	0:3b683da943e6	436	for(uint8_t i = 0; i < packetSize ; i++)
Shanglin	0:3b683da943e6	437	{
Shanglin	0:3b683da943e6	438	#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	439	pc->printf("%02X ",data[i]);
Shanglin	0:3b683da943e6	440	#endif
Shanglin	0:3b683da943e6	441	txd->putc(data[i]);
Shanglin	0:3b683da943e6	442	}
Shanglin	0:3b683da943e6	443	#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	444	pc->printf("\n");
Shanglin	0:3b683da943e6	445	#endif*/
Shanglin	0:3b683da943e6	446
Shanglin	0:3b683da943e6	447	for(uint8_t i = 0; i < packetSize ; i++)
Shanglin	0:3b683da943e6	448	{
Shanglin	0:3b683da943e6	449	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	450	serial4.putc(data[i]);
Shanglin	0:3b683da943e6	451	wait_us(100);
Shanglin	0:3b683da943e6	452	}
Shanglin	0:3b683da943e6	453	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	454	}
Shanglin	0:3b683da943e6	455	//----------------------------fonction de reception-----------------------------
Shanglin	0:3b683da943e6	456	//comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise
Shanglin	0:3b683da943e6	457	void rxPacket(uint8_t packetSize, uint8_t* data)
Shanglin	0:3b683da943e6	458	{
Shanglin	0:3b683da943e6	459	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	460	pc->printf("[RX]");
Shanglin	0:3b683da943e6	461	#endif
Shanglin	0:3b683da943e6	462	for (uint8_t i=0; i < packetSize; i++)
Shanglin	0:3b683da943e6	463	{
Shanglin	0:3b683da943e6	464	data[i] = rxd->getc();
Shanglin	0:3b683da943e6	465	#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	466	pc->printf("%02X ",data[i]);
Shanglin	0:3b683da943e6	467	#endif
Shanglin	0:3b683da943e6	468	}
Shanglin	0:3b683da943e6	469	#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	470	pc->printf("\n");
Shanglin	0:3b683da943e6	471	#endif*/
Shanglin	0:3b683da943e6	472
Shanglin	0:3b683da943e6	473	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	474	{
Shanglin	0:3b683da943e6	475	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	476	{
Shanglin	0:3b683da943e6	477	data[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	478	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	479	}
Shanglin	0:3b683da943e6	480	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	481	valueserial4=0;
Shanglin	0:3b683da943e6	482	}
Shanglin	0:3b683da943e6	483	}
Shanglin	0:3b683da943e6	484	//----------------------fonction d'effacer les erreurs--------------------------
Shanglin	0:3b683da943e6	485	void clear(uint8_t id)
Shanglin	0:3b683da943e6	486	{
Shanglin	0:3b683da943e6	487	uint8_t txBuf[11];
Shanglin	0:3b683da943e6	488	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	489	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	490	txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
Shanglin	0:3b683da943e6	491	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	492	txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
Shanglin	0:3b683da943e6	493	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	494	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	495	txBuf[7] = RAM_STATUS_ERROR; // Address
Shanglin	0:3b683da943e6	496	txBuf[8] = BYTE2; // Length
Shanglin	0:3b683da943e6	497	txBuf[9] = 0; // Clear RAM_STATUS_ERROR
Shanglin	0:3b683da943e6	498	txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
Shanglin	0:3b683da943e6	499	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	500	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	501	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
Shanglin	0:3b683da943e6	502	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	503	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	504	for(uint8_t i = 0; i < 11 ; i++)
Shanglin	0:3b683da943e6	505	{
Shanglin	0:3b683da943e6	506	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	507	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	508	wait_us(100);
Shanglin	0:3b683da943e6	509	}
Shanglin	0:3b683da943e6	510	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	511	}
Shanglin	0:3b683da943e6	512	//----------------fonction de mis a jour le couple de servo---------------------
Shanglin	0:3b683da943e6	513	void setTorque(uint8_t id, uint8_t cmdTorue)
Shanglin	0:3b683da943e6	514	{
Shanglin	0:3b683da943e6	515	uint8_t txBuf[10];
Shanglin	0:3b683da943e6	516	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	517	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	518	txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
Shanglin	0:3b683da943e6	519	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	520	txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
Shanglin	0:3b683da943e6	521	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	522	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	523	txBuf[7] = RAM_TORQUE_CONTROL; // Address
Shanglin	0:3b683da943e6	524	txBuf[8] = BYTE1; // Length
Shanglin	0:3b683da943e6	525	txBuf[9] = cmdTorue; // Torque ON
Shanglin	0:3b683da943e6	526	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	527	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	528	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
Shanglin	0:3b683da943e6	529	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	530	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	531
Shanglin	0:3b683da943e6	532	for(uint8_t i = 0; i < 10 ; i++)
Shanglin	0:3b683da943e6	533	{
Shanglin	0:3b683da943e6	534	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	535	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	536	wait_us(100);
Shanglin	0:3b683da943e6	537	}
Shanglin	0:3b683da943e6	538	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	539	}
Shanglin	0:3b683da943e6	540	//-------------fonction de controle de position pour un seul servo--------------
Shanglin	0:3b683da943e6	541	void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
Shanglin	0:3b683da943e6	542	{
Shanglin	0:3b683da943e6	543	float tempo=0;
Shanglin	0:3b683da943e6	544	//if (position > 1023) return; //1002-21
Shanglin	0:3b683da943e6	545	if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
Shanglin	0:3b683da943e6	546	tempo=playtime*0.012;
Shanglin	0:3b683da943e6	547	pos_ID = id;
Shanglin	0:3b683da943e6	548	uint8_t txBuf[12];
Shanglin	0:3b683da943e6	549	etat = pos;
Shanglin	0:3b683da943e6	550	pos_position = position;
Shanglin	0:3b683da943e6	551	pos_time = playtime;
Shanglin	0:3b683da943e6	552	pos_led = setLED;
Shanglin	0:3b683da943e6	553	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	554	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	555	txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
Shanglin	0:3b683da943e6	556	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	557	txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	558	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
Shanglin	0:3b683da943e6	559	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	560	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	561	txBuf[7] = playtime; // Playtime
Shanglin	0:3b683da943e6	562	txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	563	txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	564	txBuf[10] = POS_MODE \| setLED; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	565	txBuf[11] = id; // Servo ID
Shanglin	0:3b683da943e6	566	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	567	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	568	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
Shanglin	0:3b683da943e6	569	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	570	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	571	//txPacket(12, txBuf);
Shanglin	0:3b683da943e6	572	for(uint8_t i = 0; i < 12 ; i++)
Shanglin	0:3b683da943e6	573	{
Shanglin	0:3b683da943e6	574	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	575	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	576	wait_us(100);
Shanglin	0:3b683da943e6	577	}
Shanglin	0:3b683da943e6	578	wait(tempo);
Shanglin	0:3b683da943e6	579	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	580	}
Shanglin	0:3b683da943e6	581	//-------------fonction de controle de vitesse pour un seul servo---------------
Shanglin	0:3b683da943e6	582	//Comme je n'ai pas utilise cette fonction, je ne l'ai pas encore change
Shanglin	0:3b683da943e6	583	void velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
Shanglin	0:3b683da943e6	584	{
Shanglin	0:3b683da943e6	585	if (speed > 1023 \|\| speed < -1023) return;
Shanglin	0:3b683da943e6	586	uint8_t txBuf[12];
Shanglin	0:3b683da943e6	587	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	588	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	589	txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
Shanglin	0:3b683da943e6	590	txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	591	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
Shanglin	0:3b683da943e6	592	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	593	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	594	txBuf[7] = 0; // Playtime, unmeaningful in turn mode
Shanglin	0:3b683da943e6	595	txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	596	txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	597	txBuf[10] = TURN_MODE \| setLED; // Turn Mode and LED on/off
Shanglin	0:3b683da943e6	598	txBuf[11] = id; // Servo ID
Shanglin	0:3b683da943e6	599	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	600	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	601	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
Shanglin	0:3b683da943e6	602	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	603	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	604	txPacket(12, txBuf);
Shanglin	0:3b683da943e6	605	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	606	}
Shanglin	0:3b683da943e6	607	//--------------------fonction d'acquis d'etat d'un servo-----------------------
Shanglin	0:3b683da943e6	608	int8_t getStatus(uint8_t id)
Shanglin	0:3b683da943e6	609	{
Shanglin	0:3b683da943e6	610	uint8_t status;
Shanglin	0:3b683da943e6	611	uint8_t txBuf[7];
Shanglin	0:3b683da943e6	612	size_reponse = 9;
Shanglin	0:3b683da943e6	613
Shanglin	0:3b683da943e6	614	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	615	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	616	txBuf[2] = MIN_PACKET_SIZE; // Packet Size
Shanglin	0:3b683da943e6	617	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	618	txBuf[4] = CMD_STAT; // Status Error, Status Detail request
Shanglin	0:3b683da943e6	619	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	620	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
Shanglin	0:3b683da943e6	621	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	622
Shanglin	0:3b683da943e6	623	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	624	for(uint8_t i = 0; i < 7 ; i++)
Shanglin	0:3b683da943e6	625	{
Shanglin	0:3b683da943e6	626	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	627	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	628	wait_us(100);
Shanglin	0:3b683da943e6	629	}
Shanglin	0:3b683da943e6	630
Shanglin	0:3b683da943e6	631	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	632	uint8_t rxBuf[9];
Shanglin	0:3b683da943e6	633	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	634	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	635	{
Shanglin	0:3b683da943e6	636	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	637	{
Shanglin	0:3b683da943e6	638	rxBuf[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	639	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	640	}
Shanglin	0:3b683da943e6	641	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	642	valueserial4=0;
Shanglin	0:3b683da943e6	643	}
Shanglin	0:3b683da943e6	644
Shanglin	0:3b683da943e6	645
Shanglin	0:3b683da943e6	646	// Checksum1
Shanglin	0:3b683da943e6	647	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
Shanglin	0:3b683da943e6	648	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	649
Shanglin	0:3b683da943e6	650	return -1;
Shanglin	0:3b683da943e6	651	}
Shanglin	0:3b683da943e6	652	// Checksum2
Shanglin	0:3b683da943e6	653	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	654	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	655
Shanglin	0:3b683da943e6	656	return -1;
Shanglin	0:3b683da943e6	657	}
Shanglin	0:3b683da943e6	658	status = rxBuf[7]; // Status Error
Shanglin	0:3b683da943e6	659	//status = rxBuf[8]; // Status Detail
Shanglin	0:3b683da943e6	660
Shanglin	0:3b683da943e6	661
Shanglin	0:3b683da943e6	662	return status;
Shanglin	0:3b683da943e6	663	}
Shanglin	0:3b683da943e6	664	//------------------fonction de lire la position actuelle-----------------------
Shanglin	0:3b683da943e6	665	int16_t getPos(uint8_t id)
Shanglin	0:3b683da943e6	666	{
Shanglin	0:3b683da943e6	667	uint16_t position = 0;
Shanglin	0:3b683da943e6	668	uint8_t txBuf[9];
Shanglin	0:3b683da943e6	669	size_reponse = 13;
Shanglin	0:3b683da943e6	670
Shanglin	0:3b683da943e6	671	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	672	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	673	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
Shanglin	0:3b683da943e6	674	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	675	txBuf[4] = CMD_RAM_READ; // Command Ram Read
Shanglin	0:3b683da943e6	676	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	677	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	678	txBuf[7] = RAM_CALIBRATED_POSITION; // Address
Shanglin	0:3b683da943e6	679	txBuf[8] = BYTE2;
Shanglin	0:3b683da943e6	680	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	681	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
Shanglin	0:3b683da943e6	682	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	683
Shanglin	0:3b683da943e6	684	for(uint8_t i = 0; i < 9 ; i++)
Shanglin	0:3b683da943e6	685	{
Shanglin	0:3b683da943e6	686	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	687	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	688	wait_us(100);
Shanglin	0:3b683da943e6	689	}
Shanglin	0:3b683da943e6	690
Shanglin	0:3b683da943e6	691	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	692	uint8_t rxBuf[13];
Shanglin	0:3b683da943e6	693	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	694	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	695	{
Shanglin	0:3b683da943e6	696	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	697	{
Shanglin	0:3b683da943e6	698	rxBuf[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	699	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	700	}
Shanglin	0:3b683da943e6	701	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	702	valueserial4=0;
Shanglin	0:3b683da943e6	703	}
Shanglin	0:3b683da943e6	704
Shanglin	0:3b683da943e6	705	// Checksum1
Shanglin	0:3b683da943e6	706	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
Shanglin	0:3b683da943e6	707	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	708	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	709	pc->printf("Checksum1 fault\n");
Shanglin	0:3b683da943e6	710	#endif*/
Shanglin	0:3b683da943e6	711	return -1;
Shanglin	0:3b683da943e6	712	}
Shanglin	0:3b683da943e6	713	// Checksum2
Shanglin	0:3b683da943e6	714	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	715	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	716	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	717	pc->printf("Checksum2 fault\n");
Shanglin	0:3b683da943e6	718	#endif*/
Shanglin	0:3b683da943e6	719	return -1;
Shanglin	0:3b683da943e6	720	}
Shanglin	0:3b683da943e6	721	position = ((rxBuf[10]&0x03)<<8) \| rxBuf[9];
Shanglin	0:3b683da943e6	722	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	723	pc->printf("position = %04X(%d)\n", position, position);
Shanglin	0:3b683da943e6	724	#endif*/
Shanglin	0:3b683da943e6	725	//}
Shanglin	0:3b683da943e6	726	return position;
Shanglin	0:3b683da943e6	727	}
Shanglin	0:3b683da943e6	728	//---------------fonction d'acquis d'etat de couple d'un servo------------------
Shanglin	0:3b683da943e6	729	int8_t Get_Torque(int8_t id)
Shanglin	0:3b683da943e6	730	{
Shanglin	0:3b683da943e6	731	uint8_t txBuf[9];
Shanglin	0:3b683da943e6	732	int8_t Tor = 0;
Shanglin	0:3b683da943e6	733
Shanglin	0:3b683da943e6	734	uint8_t iv=0;
Shanglin	0:3b683da943e6	735	for(iv=0; iv<20; iv++) {
Shanglin	0:3b683da943e6	736	rx2[iv] = 0;
Shanglin	0:3b683da943e6	737	}
Shanglin	0:3b683da943e6	738
Shanglin	0:3b683da943e6	739	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	740	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	741	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
Shanglin	0:3b683da943e6	742	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	743	txBuf[4] = CMD_RAM_READ; // Command Ram Read
Shanglin	0:3b683da943e6	744	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	745	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	746	txBuf[7] = RAM_TORQUE_CONTROL;
Shanglin	0:3b683da943e6	747	txBuf[8] = BYTE1; // Length
Shanglin	0:3b683da943e6	748	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	749	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
Shanglin	0:3b683da943e6	750	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	751
Shanglin	0:3b683da943e6	752	pc.printf(" Torque ");
Shanglin	0:3b683da943e6	753
Shanglin	0:3b683da943e6	754	for(uint8_t i = 0; i < 9 ; i++)
Shanglin	0:3b683da943e6	755	{
Shanglin	0:3b683da943e6	756	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	757	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	758	wait_us(100);
Shanglin	0:3b683da943e6	759	}
Shanglin	0:3b683da943e6	760
Shanglin	0:3b683da943e6	761	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	762	uint8_t rxBuf[12];
Shanglin	0:3b683da943e6	763	//wait_ms(3);
Shanglin	0:3b683da943e6	764	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	765	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	766	{
Shanglin	0:3b683da943e6	767	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	768	{
Shanglin	0:3b683da943e6	769	rxBuf[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	770	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	771	}
Shanglin	0:3b683da943e6	772	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	773	valueserial4=0;
Shanglin	0:3b683da943e6	774	}
Shanglin	0:3b683da943e6	775
Shanglin	0:3b683da943e6	776	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
Shanglin	0:3b683da943e6	777	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	778	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	779	pc->printf("Checksum1 fault\n");
Shanglin	0:3b683da943e6	780	#endif*/
Shanglin	0:3b683da943e6	781	recevoir=0;
Shanglin	0:3b683da943e6	782	return -1;
Shanglin	0:3b683da943e6	783	}
Shanglin	0:3b683da943e6	784	// Checksum2
Shanglin	0:3b683da943e6	785	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	786	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	787	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	788	pc->printf("Checksum2 fault\n");
Shanglin	0:3b683da943e6	789	#endif*/
Shanglin	0:3b683da943e6	790	recevoir=0;
Shanglin	0:3b683da943e6	791	return -1;
Shanglin	0:3b683da943e6	792	}
Shanglin	0:3b683da943e6	793	Tor = rxBuf[9];
Shanglin	0:3b683da943e6	794	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	795	pc->printf("position = %04X(%d)\n", position, position);
Shanglin	0:3b683da943e6	796	#endif*/
Shanglin	0:3b683da943e6	797	//}
Shanglin	0:3b683da943e6	798	return Tor;
Shanglin	0:3b683da943e6	799	}
Shanglin	0:3b683da943e6	800	//---------------fonction pour lire le temperature max pour un servo------------
Shanglin	0:3b683da943e6	801	int8_t Get_Temperature_MAX(int8_t id)
Shanglin	0:3b683da943e6	802	{
Shanglin	0:3b683da943e6	803
Shanglin	0:3b683da943e6	804	uint8_t txBuf[9];
Shanglin	0:3b683da943e6	805	int8_t tempeMAX = 0;
Shanglin	0:3b683da943e6	806
Shanglin	0:3b683da943e6	807	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	808	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	809	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
Shanglin	0:3b683da943e6	810	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	811	txBuf[4] = CMD_RAM_READ; // Command Ram Read
Shanglin	0:3b683da943e6	812	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	813	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	814	txBuf[7] = RAM_MAX_TEMPERATURE;
Shanglin	0:3b683da943e6	815	txBuf[8] = BYTE1; // Length
Shanglin	0:3b683da943e6	816	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	817	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
Shanglin	0:3b683da943e6	818	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	819
Shanglin	0:3b683da943e6	820	pc.printf(" tempeMAX ");
Shanglin	0:3b683da943e6	821
Shanglin	0:3b683da943e6	822	for(uint8_t i = 0; i < 9 ; i++)
Shanglin	0:3b683da943e6	823	{
Shanglin	0:3b683da943e6	824	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	825	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	826	wait_us(100);
Shanglin	0:3b683da943e6	827	}
Shanglin	0:3b683da943e6	828
Shanglin	0:3b683da943e6	829	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	830	uint8_t rxBuf[12];
Shanglin	0:3b683da943e6	831	//wait_ms(3);
Shanglin	0:3b683da943e6	832	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	833	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	834	{
Shanglin	0:3b683da943e6	835	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	836	{
Shanglin	0:3b683da943e6	837	rxBuf[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	838	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	839	}
Shanglin	0:3b683da943e6	840	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	841	valueserial4=0;
Shanglin	0:3b683da943e6	842	}
Shanglin	0:3b683da943e6	843
Shanglin	0:3b683da943e6	844
Shanglin	0:3b683da943e6	845	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
Shanglin	0:3b683da943e6	846	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	847	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	848	pc->printf("Checksum1 fault\n");
Shanglin	0:3b683da943e6	849	#endif*/
Shanglin	0:3b683da943e6	850	recevoir=0;
Shanglin	0:3b683da943e6	851	return -1;
Shanglin	0:3b683da943e6	852	}
Shanglin	0:3b683da943e6	853	// Checksum2
Shanglin	0:3b683da943e6	854	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	855	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	856	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	857	pc->printf("Checksum2 fault\n");
Shanglin	0:3b683da943e6	858	#endif*/
Shanglin	0:3b683da943e6	859	recevoir=0;
Shanglin	0:3b683da943e6	860	return -1;
Shanglin	0:3b683da943e6	861	}
Shanglin	0:3b683da943e6	862	tempeMAX = rxBuf[9];
Shanglin	0:3b683da943e6	863	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	864	pc->printf("position = %04X(%d)\n", position, position);
Shanglin	0:3b683da943e6	865	#endif*/
Shanglin	0:3b683da943e6	866	//}
Shanglin	0:3b683da943e6	867	return tempeMAX;
Shanglin	0:3b683da943e6	868	}
Shanglin	0:3b683da943e6	869	//--------fonction de controle de position pour deux servo(same playtime)-------
Shanglin	0:3b683da943e6	870	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)
Shanglin	0:3b683da943e6	871	{
Shanglin	0:3b683da943e6	872	float tempo=0;
Shanglin	0:3b683da943e6	873	//if (position > 1023) return; //1002-21
Shanglin	0:3b683da943e6	874	if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
Shanglin	0:3b683da943e6	875	tempo=playtime*0.012;
Shanglin	0:3b683da943e6	876	uint8_t txBuf[16];
Shanglin	0:3b683da943e6	877	etat = pos;
Shanglin	0:3b683da943e6	878	pos_position = position;
Shanglin	0:3b683da943e6	879	pos_time = playtime;
Shanglin	0:3b683da943e6	880	pos_led = setLED;
Shanglin	0:3b683da943e6	881	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	882	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	883	txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
Shanglin	0:3b683da943e6	884	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	885	txBuf[3] = 254; // broadcast ID
Shanglin	0:3b683da943e6	886	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
Shanglin	0:3b683da943e6	887	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	888	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	889	txBuf[7] = playtime; // Playtime
Shanglin	0:3b683da943e6	890	txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	891	txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	892	txBuf[10] = POS_MODE \| setLED; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	893	txBuf[11] = id; // Servo ID
Shanglin	0:3b683da943e6	894	txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	895	txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	896	txBuf[14] = POS_MODE \| setLED2; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	897	txBuf[15] = id2; // Servo ID
Shanglin	0:3b683da943e6	898	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	899	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	900	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;
Shanglin	0:3b683da943e6	901	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	902	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	903
Shanglin	0:3b683da943e6	904	for(uint8_t i = 0; i < 16 ; i++)
Shanglin	0:3b683da943e6	905	{
Shanglin	0:3b683da943e6	906	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	907	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	908	wait_us(100);
Shanglin	0:3b683da943e6	909	}
Shanglin	0:3b683da943e6	910	wait(tempo);
Shanglin	0:3b683da943e6	911	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	912	}
Shanglin	0:3b683da943e6	913	//-----fonction de controle de position pour deux servo(different playtime)----- //a changer...
Shanglin	0:3b683da943e6	914	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)
Shanglin	0:3b683da943e6	915	{
Shanglin	0:3b683da943e6	916	float tempo=0;
Shanglin	0:3b683da943e6	917	//if (position > 1023) return; //1002-21
Shanglin	0:3b683da943e6	918	if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
Shanglin	0:3b683da943e6	919	if(playtime>playtime2) {
Shanglin	0:3b683da943e6	920	tempo=playtime*0.012;
Shanglin	0:3b683da943e6	921	} else if(playtime<playtime2) {
Shanglin	0:3b683da943e6	922	tempo=playtime2*0.012;
Shanglin	0:3b683da943e6	923	}
Shanglin	0:3b683da943e6	924	uint8_t txBuf[17];
Shanglin	0:3b683da943e6	925	etat = pos;
Shanglin	0:3b683da943e6	926	pos_position = position;
Shanglin	0:3b683da943e6	927	pos_time = playtime;
Shanglin	0:3b683da943e6	928	pos_led = setLED;
Shanglin	0:3b683da943e6	929	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	930	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	931	txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
Shanglin	0:3b683da943e6	932	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	933	txBuf[3] = 254; // broadcast ID
Shanglin	0:3b683da943e6	934	txBuf[4] = CMD_I_JOG; // Command I JOG
Shanglin	0:3b683da943e6	935	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	936	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	937	txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	938	txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	939	txBuf[9] = POS_MODE \| setLED; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	940	txBuf[10] = id; // Servo ID
Shanglin	0:3b683da943e6	941	txBuf[11] = playtime; // Playtime
Shanglin	0:3b683da943e6	942	txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	943	txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	944	txBuf[14] = POS_MODE \| setLED2; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	945	txBuf[15] = id2; // Servo ID
Shanglin	0:3b683da943e6	946	txBuf[16] = playtime2; // Playtime
Shanglin	0:3b683da943e6	947	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	948	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	949	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;
Shanglin	0:3b683da943e6	950	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	951	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	952	//txPacket(12, txBuf);
Shanglin	0:3b683da943e6	953
Shanglin	0:3b683da943e6	954	for(uint8_t i = 0; i < 17 ; i++)
Shanglin	0:3b683da943e6	955	{
Shanglin	0:3b683da943e6	956	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	957	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	958	wait_us(100);
Shanglin	0:3b683da943e6	959	}
Shanglin	0:3b683da943e6	960	wait(tempo);
Shanglin	0:3b683da943e6	961	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	962	}
Shanglin	0:3b683da943e6	963	//-----fonction de controle de position pour plusieurs servo(same playtime)-----
Shanglin	0:3b683da943e6	964	void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos) // uint16_t position, uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	965	{
Shanglin	0:3b683da943e6	966	float tempo=0;
Shanglin	0:3b683da943e6	967	uint8_t taille = 0,i = 0,idata = 0, ipos = 0;
Shanglin	0:3b683da943e6	968	//if (position > 1023) return; //1002-21
Shanglin	0:3b683da943e6	969	if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
Shanglin	0:3b683da943e6	970	tempo=playtime*0.012;
Shanglin	0:3b683da943e6	971	taille = 7 + 1 + 4 * nb_servo;
Shanglin	0:3b683da943e6	972	nombre_servo = nb_servo;
Shanglin	0:3b683da943e6	973	pos_time = playtime;
Shanglin	0:3b683da943e6	974	uint8_t txBuf[taille];
Shanglin	0:3b683da943e6	975	etat = pos_mul_complex;
Shanglin	0:3b683da943e6	976
Shanglin	0:3b683da943e6	977	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	978	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	979	txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size
Shanglin	0:3b683da943e6	980	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	981	txBuf[3] = 254; // broadcast ID
Shanglin	0:3b683da943e6	982	txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
Shanglin	0:3b683da943e6	983	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	984	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	985	txBuf[7] = playtime; // Playtime
Shanglin	0:3b683da943e6	986
Shanglin	0:3b683da943e6	987	for(i=0; i<nb_servo; i++) {
Shanglin	0:3b683da943e6	988	txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	989	txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	990	position_servo_mul[ipos] = pos[ipos];
Shanglin	0:3b683da943e6	991	ipos++;
Shanglin	0:3b683da943e6	992	txBuf[10+i*4] = POS_MODE \| data[idata]; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	993	data_servo_mul[idata] = data[idata];
Shanglin	0:3b683da943e6	994	idata++;
Shanglin	0:3b683da943e6	995	txBuf[11+i*4] = data[idata]; // Servo ID
Shanglin	0:3b683da943e6	996	data_servo_mul[idata] = data[idata];
Shanglin	0:3b683da943e6	997	idata++;
Shanglin	0:3b683da943e6	998	}
Shanglin	0:3b683da943e6	999
Shanglin	0:3b683da943e6	1000	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	1001	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	1002	txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7];
Shanglin	0:3b683da943e6	1003
Shanglin	0:3b683da943e6	1004	for(i=1; i<(taille-7); i++) {
Shanglin	0:3b683da943e6	1005	txBuf[5]=txBuf[5]^txBuf[7+i];
Shanglin	0:3b683da943e6	1006	}
Shanglin	0:3b683da943e6	1007	txBuf[5] = txBuf[5]& 0xFE;
Shanglin	0:3b683da943e6	1008	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	1009
Shanglin	0:3b683da943e6	1010	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	1011
Shanglin	0:3b683da943e6	1012	for(uint8_t i = 0; i < taille ; i++)
Shanglin	0:3b683da943e6	1013	{
Shanglin	0:3b683da943e6	1014	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	1015	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	1016	wait_us(100);
Shanglin	0:3b683da943e6	1017	}
Shanglin	0:3b683da943e6	1018	//wait(tempo);
Shanglin	0:3b683da943e6	1019	//wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	1020	}
Shanglin	0:3b683da943e6	1021	//--fonction de controle de position pour plusieurs servo(different playtime)---
Shanglin	0:3b683da943e6	1022	void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos) // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime
Shanglin	0:3b683da943e6	1023	{
Shanglin	0:3b683da943e6	1024	float tempo=0;
Shanglin	0:3b683da943e6	1025	uint8_t Max_playtime = 0;
Shanglin	0:3b683da943e6	1026	uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0;
Shanglin	0:3b683da943e6	1027	//if (position > 1023) return; //1002-21
Shanglin	0:3b683da943e6	1028	//if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
Shanglin	0:3b683da943e6	1029
Shanglin	0:3b683da943e6	1030	for(iplay_time=0; iplay_time<nb_servo; iplay_time++) {
Shanglin	0:3b683da943e6	1031	if(Max_playtime<data[2+3*iplay_time]) {
Shanglin	0:3b683da943e6	1032	Max_playtime=data[2+3*iplay_time];
Shanglin	0:3b683da943e6	1033	}
Shanglin	0:3b683da943e6	1034	}
Shanglin	0:3b683da943e6	1035	tempo=Max_playtime*0.012;
Shanglin	0:3b683da943e6	1036	taille = 7 + 5 * nb_servo;
Shanglin	0:3b683da943e6	1037	nombre_servo = nb_servo;
Shanglin	0:3b683da943e6	1038	uint8_t txBuf[taille];
Shanglin	0:3b683da943e6	1039	etat = pos_mul_complex_different;
Shanglin	0:3b683da943e6	1040
Shanglin	0:3b683da943e6	1041	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1042	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1043	txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size
Shanglin	0:3b683da943e6	1044	//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
Shanglin	0:3b683da943e6	1045	txBuf[3] = 254; // broadcast ID
Shanglin	0:3b683da943e6	1046	txBuf[4] = CMD_I_JOG; // Command I JOG (0x06)
Shanglin	0:3b683da943e6	1047	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	1048	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	1049
Shanglin	0:3b683da943e6	1050	for(i=0; i<nb_servo; i++) {
Shanglin	0:3b683da943e6	1051	txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
Shanglin	0:3b683da943e6	1052	txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
Shanglin	0:3b683da943e6	1053	position_servo_mul_different[ipos] = pos[ipos];
Shanglin	0:3b683da943e6	1054	ipos++;
Shanglin	0:3b683da943e6	1055	txBuf[9+i*5] = POS_MODE \| data[idata]; // Pos Mode and LED on/off
Shanglin	0:3b683da943e6	1056	data_servo_mul_different[idata] = data[idata];
Shanglin	0:3b683da943e6	1057	idata++;
Shanglin	0:3b683da943e6	1058	txBuf[10+i*5] = data[idata]; // Servo ID
Shanglin	0:3b683da943e6	1059	data_servo_mul_different[idata] = data[idata];
Shanglin	0:3b683da943e6	1060	idata++;
Shanglin	0:3b683da943e6	1061	txBuf[11+i*5] = data[idata]; // Playtime
Shanglin	0:3b683da943e6	1062	data_servo_mul_different[idata] = data[idata];
Shanglin	0:3b683da943e6	1063	idata++;
Shanglin	0:3b683da943e6	1064	}
Shanglin	0:3b683da943e6	1065
Shanglin	0:3b683da943e6	1066	// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
Shanglin	0:3b683da943e6	1067	// Checksum2 = (~Checksum1)&0xFE
Shanglin	0:3b683da943e6	1068	txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4];
Shanglin	0:3b683da943e6	1069
Shanglin	0:3b683da943e6	1070	for(i=1; i<(taille-6); i++) {
Shanglin	0:3b683da943e6	1071	txBuf[5]=txBuf[5]^txBuf[6+i];
Shanglin	0:3b683da943e6	1072	}
Shanglin	0:3b683da943e6	1073	txBuf[5] = txBuf[5]& 0xFE;
Shanglin	0:3b683da943e6	1074	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	1075
Shanglin	0:3b683da943e6	1076	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	1077
Shanglin	0:3b683da943e6	1078	for(uint8_t i = 0; i < taille ; i++)
Shanglin	0:3b683da943e6	1079	{
Shanglin	0:3b683da943e6	1080	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	1081	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	1082	wait_us(100);
Shanglin	0:3b683da943e6	1083	}
Shanglin	0:3b683da943e6	1084	wait(tempo);
Shanglin	0:3b683da943e6	1085	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	1086	}
Shanglin	0:3b683da943e6	1087	//---------------fonction pour lire la tension min pour un servo----------------
Shanglin	0:3b683da943e6	1088	int8_t Get_Tension_MIN(int8_t id)
Shanglin	0:3b683da943e6	1089	{
Shanglin	0:3b683da943e6	1090
Shanglin	0:3b683da943e6	1091	uint8_t txBuf[9];
Shanglin	0:3b683da943e6	1092	int8_t tensionMIN = 0;
Shanglin	0:3b683da943e6	1093
Shanglin	0:3b683da943e6	1094	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1095	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1096	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
Shanglin	0:3b683da943e6	1097	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	1098	txBuf[4] = CMD_RAM_READ; // Command Ram Read
Shanglin	0:3b683da943e6	1099	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	1100	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	1101	txBuf[7] = RAM_MIN_VOLTAGE;
Shanglin	0:3b683da943e6	1102	txBuf[8] = BYTE1; // Length
Shanglin	0:3b683da943e6	1103	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	1104	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
Shanglin	0:3b683da943e6	1105	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	1106
Shanglin	0:3b683da943e6	1107	pc.printf(" tensionMIN ");
Shanglin	0:3b683da943e6	1108
Shanglin	0:3b683da943e6	1109	for(uint8_t i = 0; i < 9 ; i++)
Shanglin	0:3b683da943e6	1110	{
Shanglin	0:3b683da943e6	1111	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	1112	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	1113	wait_us(100);
Shanglin	0:3b683da943e6	1114	}
Shanglin	0:3b683da943e6	1115
Shanglin	0:3b683da943e6	1116	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	1117	uint8_t rxBuf[12];
Shanglin	0:3b683da943e6	1118	//wait_ms(3);
Shanglin	0:3b683da943e6	1119	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	1120	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	1121	{
Shanglin	0:3b683da943e6	1122	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	1123	{
Shanglin	0:3b683da943e6	1124	rxBuf[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	1125	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	1126	}
Shanglin	0:3b683da943e6	1127	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	1128	valueserial4=0;
Shanglin	0:3b683da943e6	1129	}
Shanglin	0:3b683da943e6	1130
Shanglin	0:3b683da943e6	1131	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
Shanglin	0:3b683da943e6	1132	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	1133	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	1134	pc->printf("Checksum1 fault\n");
Shanglin	0:3b683da943e6	1135	#endif*/
Shanglin	0:3b683da943e6	1136	recevoir=0;
Shanglin	0:3b683da943e6	1137	return -1;
Shanglin	0:3b683da943e6	1138	}
Shanglin	0:3b683da943e6	1139	// Checksum2
Shanglin	0:3b683da943e6	1140	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	1141	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	1142	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	1143	pc->printf("Checksum2 fault\n");
Shanglin	0:3b683da943e6	1144	#endif*/
Shanglin	0:3b683da943e6	1145	recevoir=0;
Shanglin	0:3b683da943e6	1146	return -1;
Shanglin	0:3b683da943e6	1147	}
Shanglin	0:3b683da943e6	1148	tensionMIN = rxBuf[9];
Shanglin	0:3b683da943e6	1149	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	1150	pc->printf("position = %04X(%d)\n", position, position);
Shanglin	0:3b683da943e6	1151	#endif*/
Shanglin	0:3b683da943e6	1152	//}
Shanglin	0:3b683da943e6	1153	return tensionMIN;
Shanglin	0:3b683da943e6	1154	}
Shanglin	0:3b683da943e6	1155	//-------------fonction pour controle la tension min pour un servo--------------
Shanglin	0:3b683da943e6	1156	void Set_Tension_MIN(int8_t id,uint8_t Tension_Min)
Shanglin	0:3b683da943e6	1157	{
Shanglin	0:3b683da943e6	1158	uint8_t txBuf[10];
Shanglin	0:3b683da943e6	1159
Shanglin	0:3b683da943e6	1160	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1161	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1162	txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
Shanglin	0:3b683da943e6	1163	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	1164	txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
Shanglin	0:3b683da943e6	1165	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	1166	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	1167	txBuf[7] = RAM_MIN_VOLTAGE;
Shanglin	0:3b683da943e6	1168	txBuf[8] = BYTE1; // Length
Shanglin	0:3b683da943e6	1169	txBuf[9] = Tension_Min;
Shanglin	0:3b683da943e6	1170	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	1171	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
Shanglin	0:3b683da943e6	1172	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	1173
Shanglin	0:3b683da943e6	1174	pc.printf(" tensionMIN ");
Shanglin	0:3b683da943e6	1175	for(uint8_t i = 0; i < 10 ; i++)
Shanglin	0:3b683da943e6	1176	{
Shanglin	0:3b683da943e6	1177	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	1178	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	1179	wait_us(100);
Shanglin	0:3b683da943e6	1180	}
Shanglin	0:3b683da943e6	1181	//wait_ms(3);
Shanglin	0:3b683da943e6	1182	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	1183	}
Shanglin	0:3b683da943e6	1184	//------------fonction pour lire la tension acttuelle pour un servo-------------
Shanglin	0:3b683da943e6	1185	int8_t Get_Tension_actuelle(int8_t id)
Shanglin	0:3b683da943e6	1186	{
Shanglin	0:3b683da943e6	1187
Shanglin	0:3b683da943e6	1188	uint8_t txBuf[9];
Shanglin	0:3b683da943e6	1189	int8_t tension = 0;
Shanglin	0:3b683da943e6	1190
Shanglin	0:3b683da943e6	1191	txBuf[0] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1192	txBuf[1] = HEADER; // Packet Header (0xFF)
Shanglin	0:3b683da943e6	1193	txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
Shanglin	0:3b683da943e6	1194	txBuf[3] = id; // Servo ID
Shanglin	0:3b683da943e6	1195	txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03)
Shanglin	0:3b683da943e6	1196	txBuf[5] = 0; // Checksum1
Shanglin	0:3b683da943e6	1197	txBuf[6] = 0; // Checksum2
Shanglin	0:3b683da943e6	1198	txBuf[7] = RAM_VOLTAGE;
Shanglin	0:3b683da943e6	1199	txBuf[8] = BYTE2; // Length
Shanglin	0:3b683da943e6	1200	// Check Sum1 and Check Sum2
Shanglin	0:3b683da943e6	1201	txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
Shanglin	0:3b683da943e6	1202	txBuf[6] = (~txBuf[5])&0xFE;
Shanglin	0:3b683da943e6	1203
Shanglin	0:3b683da943e6	1204	pc.printf(" tension ");
Shanglin	0:3b683da943e6	1205
Shanglin	0:3b683da943e6	1206	for(uint8_t i = 0; i < 9 ; i++)
Shanglin	0:3b683da943e6	1207	{
Shanglin	0:3b683da943e6	1208	while(serial4.writeable() == 0);
Shanglin	0:3b683da943e6	1209	serial4.putc(txBuf[i]);
Shanglin	0:3b683da943e6	1210	wait_us(100);
Shanglin	0:3b683da943e6	1211	}
Shanglin	0:3b683da943e6	1212
Shanglin	0:3b683da943e6	1213	// send packet (mbed -> herkulex)
Shanglin	0:3b683da943e6	1214	uint8_t rxBuf[13];
Shanglin	0:3b683da943e6	1215	//wait_ms(3);
Shanglin	0:3b683da943e6	1216	wait_ms(TEMPO_R);
Shanglin	0:3b683da943e6	1217	if(flag_serial4_receive)
Shanglin	0:3b683da943e6	1218	{
Shanglin	0:3b683da943e6	1219	for(unsigned char i4=0;i4<Size_trame; i4++)
Shanglin	0:3b683da943e6	1220	{
Shanglin	0:3b683da943e6	1221	rxBuf[i4] = char_receive_serial4[i4];
Shanglin	0:3b683da943e6	1222	pc.printf("%d ",(int)char_receive_serial4[i4]);
Shanglin	0:3b683da943e6	1223	}
Shanglin	0:3b683da943e6	1224	flag_serial4_receive=0;
Shanglin	0:3b683da943e6	1225	valueserial4=0;
Shanglin	0:3b683da943e6	1226	}
Shanglin	0:3b683da943e6	1227
Shanglin	0:3b683da943e6	1228
Shanglin	0:3b683da943e6	1229	uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
Shanglin	0:3b683da943e6	1230	if (chksum1 != rxBuf[5]) {
Shanglin	0:3b683da943e6	1231	/*#ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	1232	pc->printf("Checksum1 fault\n");
Shanglin	0:3b683da943e6	1233	#endif*/
Shanglin	0:3b683da943e6	1234	recevoir=0;
Shanglin	0:3b683da943e6	1235	return -1;
Shanglin	0:3b683da943e6	1236	}
Shanglin	0:3b683da943e6	1237	// Checksum2
Shanglin	0:3b683da943e6	1238	uint8_t chksum2 = (~rxBuf[5]&0xFE);
Shanglin	0:3b683da943e6	1239	if (chksum2 != rxBuf[6]) {
Shanglin	0:3b683da943e6	1240	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	1241	pc->printf("Checksum2 fault\n");
Shanglin	0:3b683da943e6	1242	#endif*/
Shanglin	0:3b683da943e6	1243	recevoir=0;
Shanglin	0:3b683da943e6	1244	return -1;
Shanglin	0:3b683da943e6	1245	}
Shanglin	0:3b683da943e6	1246	tension = rxBuf[9];
Shanglin	0:3b683da943e6	1247	/* #ifdef HERKULEX_DEBUG
Shanglin	0:3b683da943e6	1248	pc->printf("position = %04X(%d)\n", position, position);
Shanglin	0:3b683da943e6	1249	#endif*/
Shanglin	0:3b683da943e6	1250	//}
Shanglin	0:3b683da943e6	1251	return tension;
Shanglin	0:3b683da943e6	1252	}
Shanglin	0:3b683da943e6	1253
Shanglin	0:3b683da943e6	1254	//-----------------deplacement des cubes---------------------
Shanglin	0:3b683da943e6	1255	void deplacement_cubes(uint8_t poi_init, uint8_t poi_fini,int8_t ID,int8_t ID2,int8_t ID3,int8_t ID4,PinName Pompe_pin)
Shanglin	0:3b683da943e6	1256	{
Shanglin	0:3b683da943e6	1257	PwmOut Pompe(Pompe_pin);
Shanglin	0:3b683da943e6	1258	switch(poi_init) {
Shanglin	0:3b683da943e6	1259	case 5: //verifie X
Shanglin	0:3b683da943e6	1260	//uint8_t servo1[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1261	//uint8_t servo2[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1262	uint8_t servo3[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1263	uint8_t servo4[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1264	uint8_t servo5[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1265	//uint16_t position1[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1266	//uint16_t position2[3] = {512, 512, 788};
Shanglin	0:3b683da943e6	1267	uint16_t position3[4] = {374, 926, 788, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1268	uint16_t position6[4] = {481, 950, 714, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1269	uint16_t position4[4] = {650, 952, 511, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1270	uint16_t position5[4] = {451, 944, 720, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1271
Shanglin	0:3b683da943e6	1272	/*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo1, position1);
Shanglin	0:3b683da943e6	1273	verifacation();
Shanglin	0:3b683da943e6	1274	positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo2, position2);
Shanglin	0:3b683da943e6	1275	verifacation();*/
Shanglin	0:3b683da943e6	1276	positionControl_Mul_ensemble_complex(4,V_b,servo3, position3);
Shanglin	0:3b683da943e6	1277	verifacation();
Shanglin	0:3b683da943e6	1278	positionControl_Mul_ensemble_complex(4,V_b,servo3, position6);
Shanglin	0:3b683da943e6	1279	verifacation();
Shanglin	0:3b683da943e6	1280	positionControl_Mul_ensemble_complex(4,V_h,servo4, position4);
Shanglin	0:3b683da943e6	1281	verifacation();
Shanglin	0:3b683da943e6	1282	wait(0.3);
Shanglin	0:3b683da943e6	1283	Pompe = 1;
Shanglin	0:3b683da943e6	1284	wait(0.1);
Shanglin	0:3b683da943e6	1285	positionControl_Mul_ensemble_complex(4,V_m,servo5, position5);
Shanglin	0:3b683da943e6	1286	verifacation();
Shanglin	0:3b683da943e6	1287
Shanglin	0:3b683da943e6	1288	break;
Shanglin	0:3b683da943e6	1289	case 3: //verifie X
Shanglin	0:3b683da943e6	1290	//uint8_t servo31[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1291	//uint8_t servo32[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1292	uint8_t servo33[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1293	uint8_t servo34[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1294	uint8_t servo35[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1295	//uint16_t position31[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1296	//uint16_t position32[3] = {512, 512, 788};
Shanglin	0:3b683da943e6	1297	uint16_t position33[4] = {374, 926, 788, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1298	uint16_t position34[4] = {673, 837, 595, Pos_Engrenage_centre}; //647, 842, 604
Shanglin	0:3b683da943e6	1299	uint16_t position36[4] = {547, 845, 702, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1300	uint16_t position35[4] = {451, 944, 720, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1301
Shanglin	0:3b683da943e6	1302	//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo31, position31);
Shanglin	0:3b683da943e6	1303	//verifacation();
Shanglin	0:3b683da943e6	1304	//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo32, position32);
Shanglin	0:3b683da943e6	1305	//verifacation();
Shanglin	0:3b683da943e6	1306	positionControl_Mul_ensemble_complex(4,V_b,servo33, position33);
Shanglin	0:3b683da943e6	1307	verifacation();
Shanglin	0:3b683da943e6	1308	positionControl_Mul_ensemble_complex(4,V_h,servo34, position34);
Shanglin	0:3b683da943e6	1309	verifacation();
Shanglin	0:3b683da943e6	1310	wait(0.3);
Shanglin	0:3b683da943e6	1311	Pompe = 1;
Shanglin	0:3b683da943e6	1312	wait(0.1);
Shanglin	0:3b683da943e6	1313	positionControl_Mul_ensemble_complex(4,V_m,servo33, position36);
Shanglin	0:3b683da943e6	1314	verifacation();
Shanglin	0:3b683da943e6	1315	positionControl_Mul_ensemble_complex(4,V_m,servo35, position35);
Shanglin	0:3b683da943e6	1316	verifacation();
Shanglin	0:3b683da943e6	1317
Shanglin	0:3b683da943e6	1318	break;
Shanglin	0:3b683da943e6	1319	case 2:
Shanglin	0:3b683da943e6	1320	//uint8_t servo20[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1321	//uint8_t servo21[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1322	//uint8_t servo22[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1323	uint8_t servo23[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1324	uint8_t servo24[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1325	uint8_t servo25[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1326	//uint16_t position20[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1327	//uint16_t position21[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1328	//uint16_t position22[3] = {512, 512, 788};
Shanglin	0:3b683da943e6	1329	uint16_t position23[4] = {374, 926, 788, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1330	uint16_t position24[4] = {673, 837, 595, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1331	uint16_t position25[4] = {451, 944, 720, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1332
Shanglin	0:3b683da943e6	1333	/*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo20, position20);
Shanglin	0:3b683da943e6	1334	verifacation();
Shanglin	0:3b683da943e6	1335	wait(0.2);
Shanglin	0:3b683da943e6	1336	positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo21, position21);
Shanglin	0:3b683da943e6	1337	verifacation();
Shanglin	0:3b683da943e6	1338	positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo22, position22);
Shanglin	0:3b683da943e6	1339	verifacation();*/
Shanglin	0:3b683da943e6	1340	positionControl_Mul_ensemble_complex(4,V_b,servo23, position23);
Shanglin	0:3b683da943e6	1341	verifacation();
Shanglin	0:3b683da943e6	1342	positionControl_Mul_ensemble_complex(4,V_h,servo24, position24);
Shanglin	0:3b683da943e6	1343	verifacation();
Shanglin	0:3b683da943e6	1344	wait(0.3);
Shanglin	0:3b683da943e6	1345	Pompe = 1;
Shanglin	0:3b683da943e6	1346	wait(0.1);
Shanglin	0:3b683da943e6	1347	positionControl_Mul_ensemble_complex(4,V_m,servo25, position25);
Shanglin	0:3b683da943e6	1348	verifacation();
Shanglin	0:3b683da943e6	1349
Shanglin	0:3b683da943e6	1350	break;
Shanglin	0:3b683da943e6	1351	case 4:
Shanglin	0:3b683da943e6	1352	//uint8_t servo40[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1353	//uint8_t servo41[6] = {GLED_ON, ID4, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1354	//uint8_t servo42[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1355	uint8_t servo43[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1356	uint8_t servo44[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1357	uint8_t servo45[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1358	//uint16_t position40[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1359	//uint16_t position41[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1360	//uint16_t position42[3] = {512, 512, 788};
Shanglin	0:3b683da943e6	1361	uint16_t position43[4] = {374, 926, 788, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1362	uint16_t position44[4] = {673, 837, 595, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1363	uint16_t position45[4] = {451, 944, 720, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1364
Shanglin	0:3b683da943e6	1365	/*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo40, position40);
Shanglin	0:3b683da943e6	1366	verifacation();
Shanglin	0:3b683da943e6	1367	wait(0.2);
Shanglin	0:3b683da943e6	1368	positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo41, position41);
Shanglin	0:3b683da943e6	1369	verifacation();
Shanglin	0:3b683da943e6	1370	positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo42, position42);
Shanglin	0:3b683da943e6	1371	verifacation();*/
Shanglin	0:3b683da943e6	1372	positionControl_Mul_ensemble_complex(4,V_b,servo43, position43);
Shanglin	0:3b683da943e6	1373	verifacation();
Shanglin	0:3b683da943e6	1374	positionControl_Mul_ensemble_complex(4,V_h,servo44, position44);
Shanglin	0:3b683da943e6	1375	verifacation();
Shanglin	0:3b683da943e6	1376	wait(0.3);
Shanglin	0:3b683da943e6	1377	Pompe = 1;
Shanglin	0:3b683da943e6	1378	wait(0.1);
Shanglin	0:3b683da943e6	1379	positionControl_Mul_ensemble_complex(4,V_m,servo45, position45);
Shanglin	0:3b683da943e6	1380	verifacation();
Shanglin	0:3b683da943e6	1381
Shanglin	0:3b683da943e6	1382	break;
Shanglin	0:3b683da943e6	1383	case 1: //verifie X
Shanglin	0:3b683da943e6	1384	//uint8_t servo11[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1385	//uint8_t servo12[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
Shanglin	0:3b683da943e6	1386	uint8_t servo13[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1387	uint8_t servo14[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1388	uint8_t servo15[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1389	uint8_t servo16[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
Shanglin	0:3b683da943e6	1390	//uint16_t position11[3] = {512, 236, 236};
Shanglin	0:3b683da943e6	1391	//uint16_t position12[3] = {512, 512, 788};
Shanglin	0:3b683da943e6	1392	uint16_t position13[4] = {374, 926, 788, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1393	uint16_t position17[4] = {658, 657, 790, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1394	uint16_t position14[4] = {731, 675, 695, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1395	uint16_t position15[4] = {658, 657, 790, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1396	uint16_t position16[4] = {417, 885, 796, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1397
Shanglin	0:3b683da943e6	1398
Shanglin	0:3b683da943e6	1399	//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo11, position11);
Shanglin	0:3b683da943e6	1400	//verifacation();
Shanglin	0:3b683da943e6	1401	//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo12, position12);
Shanglin	0:3b683da943e6	1402	//verifacation();
Shanglin	0:3b683da943e6	1403	positionControl_Mul_ensemble_complex(4,V_b,servo13, position13);
Shanglin	0:3b683da943e6	1404	verifacation();
Shanglin	0:3b683da943e6	1405	positionControl_Mul_ensemble_complex(4,V_m,servo13, position17);
Shanglin	0:3b683da943e6	1406	verifacation();
Shanglin	0:3b683da943e6	1407	positionControl_Mul_ensemble_complex(4,V_h,servo14, position14);
Shanglin	0:3b683da943e6	1408	verifacation();
Shanglin	0:3b683da943e6	1409	wait(0.3);
Shanglin	0:3b683da943e6	1410	Pompe = 1;
Shanglin	0:3b683da943e6	1411	wait(0.1);
Shanglin	0:3b683da943e6	1412	positionControl_Mul_ensemble_complex(4,V_h,servo15, position15);
Shanglin	0:3b683da943e6	1413	verifacation();
Shanglin	0:3b683da943e6	1414	//wait(2);
Shanglin	0:3b683da943e6	1415	positionControl_Mul_ensemble_complex(4,V_h,servo16, position16);
Shanglin	0:3b683da943e6	1416	verifacation();
Shanglin	0:3b683da943e6	1417
Shanglin	0:3b683da943e6	1418	break;
Shanglin	0:3b683da943e6	1419
Shanglin	0:3b683da943e6	1420	/*case 22:
Shanglin	0:3b683da943e6	1421	uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1422	uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1423	uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1424	uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1425
Shanglin	0:3b683da943e6	1426	uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1427	uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1428	uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1429	uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_gauche};
Shanglin	0:3b683da943e6	1430
Shanglin	0:3b683da943e6	1431	positionControl_Mul_ensemble_complex(4,V_m,servo221, position221);
Shanglin	0:3b683da943e6	1432	verifacation();
Shanglin	0:3b683da943e6	1433	positionControl_Mul_ensemble_complex(4,V_h,servo222, position222);
Shanglin	0:3b683da943e6	1434	verifacation();
Shanglin	0:3b683da943e6	1435	positionControl_Mul_ensemble_complex(4,V_b,servo223, position223);
Shanglin	0:3b683da943e6	1436	verifacation();
Shanglin	0:3b683da943e6	1437	wait(0.5);
Shanglin	0:3b683da943e6	1438	Pompe = 1;
Shanglin	0:3b683da943e6	1439	wait(0.5);
Shanglin	0:3b683da943e6	1440	positionControl_Mul_ensemble_complex(4,V_b,servo224, position224);
Shanglin	0:3b683da943e6	1441	verifacation();
Shanglin	0:3b683da943e6	1442
Shanglin	0:3b683da943e6	1443	break;
Shanglin	0:3b683da943e6	1444
Shanglin	0:3b683da943e6	1445	case 42:
Shanglin	0:3b683da943e6	1446	uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1447	uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1448	uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1449	uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1450
Shanglin	0:3b683da943e6	1451	uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1452	uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1453	uint16_t position423[4] = {553, 819, 733, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1454	uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_droit};
Shanglin	0:3b683da943e6	1455
Shanglin	0:3b683da943e6	1456	positionControl_Mul_ensemble_complex(4,V_m,servo421, position421);
Shanglin	0:3b683da943e6	1457	verifacation();
Shanglin	0:3b683da943e6	1458	positionControl_Mul_ensemble_complex(4,V_h,servo422, position422);
Shanglin	0:3b683da943e6	1459	verifacation();
Shanglin	0:3b683da943e6	1460	positionControl_Mul_ensemble_complex(4,V_b,servo423, position423);
Shanglin	0:3b683da943e6	1461	verifacation();
Shanglin	0:3b683da943e6	1462	wait(0.5);
Shanglin	0:3b683da943e6	1463	Pompe = 1;
Shanglin	0:3b683da943e6	1464	wait(0.5);
Shanglin	0:3b683da943e6	1465	positionControl_Mul_ensemble_complex(4,V_b,servo424, position424);
Shanglin	0:3b683da943e6	1466	verifacation();
Shanglin	0:3b683da943e6	1467
Shanglin	0:3b683da943e6	1468	break;*/
Shanglin	0:3b683da943e6	1469	}
Shanglin	0:3b683da943e6	1470
Shanglin	0:3b683da943e6	1471	switch(poi_fini) {
Shanglin	0:3b683da943e6	1472	/*case 11:
Shanglin	0:3b683da943e6	1473	uint8_t servo101[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1474	uint8_t servo102[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1475	uint8_t servo103[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1476	uint8_t servo104[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1477
Shanglin	0:3b683da943e6	1478	uint16_t position101[4] = {663, 615, 820, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1479	uint16_t position102[4] = {742, 542, 819, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1480	uint16_t position103[4] = {715, 660, 719, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1481	uint16_t position104[4] = {418, 885, 789, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1482
Shanglin	0:3b683da943e6	1483	positionControl_Mul_ensemble_complex(4,V_b,servo101, position101);
Shanglin	0:3b683da943e6	1484	verifacation();
Shanglin	0:3b683da943e6	1485	positionControl_Mul_ensemble_complex(4,V_m,servo102, position102);
Shanglin	0:3b683da943e6	1486	verifacation();
Shanglin	0:3b683da943e6	1487	positionControl_Mul_ensemble_complex(4,V_h,servo103, position103);
Shanglin	0:3b683da943e6	1488	verifacation();
Shanglin	0:3b683da943e6	1489	wait(0.5);
Shanglin	0:3b683da943e6	1490	Pompe = 0;
Shanglin	0:3b683da943e6	1491	wait(0.5);
Shanglin	0:3b683da943e6	1492	positionControl_Mul_ensemble_complex(4,V_b,servo104, position104);
Shanglin	0:3b683da943e6	1493	verifacation();
Shanglin	0:3b683da943e6	1494
Shanglin	0:3b683da943e6	1495	break;*/
Shanglin	0:3b683da943e6	1496
Shanglin	0:3b683da943e6	1497	case 12:
Shanglin	0:3b683da943e6	1498	uint8_t servo121[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1499	uint8_t servo122[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1500	uint8_t servo123[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1501	uint8_t servo124[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1502
Shanglin	0:3b683da943e6	1503	uint16_t position121[4] = {660, 620, 828, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1504	uint16_t position122[4] = {659, 669, 763, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1505	uint16_t position123[4] = {532, 570, 835, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1506	uint16_t position124[4] = {418, 885, 789, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1507
Shanglin	0:3b683da943e6	1508	positionControl_Mul_ensemble_complex(4,V_m,servo121, position121);
Shanglin	0:3b683da943e6	1509	verifacation();
Shanglin	0:3b683da943e6	1510	positionControl_Mul_ensemble_complex(4,V_h,servo122, position122);
Shanglin	0:3b683da943e6	1511	verifacation();
Shanglin	0:3b683da943e6	1512	wait(0.1);
Shanglin	0:3b683da943e6	1513	Pompe = 0;
Shanglin	0:3b683da943e6	1514	wait(0.2);
Shanglin	0:3b683da943e6	1515
Shanglin	0:3b683da943e6	1516	positionControl_Mul_ensemble_complex(4,V_b,servo123, position123);
Shanglin	0:3b683da943e6	1517	verifacation();
Shanglin	0:3b683da943e6	1518	positionControl_Mul_ensemble_complex(4,V_b,servo124, position124);
Shanglin	0:3b683da943e6	1519	verifacation();
Shanglin	0:3b683da943e6	1520
Shanglin	0:3b683da943e6	1521	break;
Shanglin	0:3b683da943e6	1522
Shanglin	0:3b683da943e6	1523	case 13:
Shanglin	0:3b683da943e6	1524	uint8_t servo131[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1525	uint8_t servo132[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1526	uint8_t servo133[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1527	uint8_t servo134[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1528
Shanglin	0:3b683da943e6	1529	uint16_t position131[4] = {552, 649, 853, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1530	uint16_t position132[4] = {647, 589, 813, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1531	uint16_t position133[4] = {552, 649, 853, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1532	uint16_t position134[4] = {418, 885, 789, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1533
Shanglin	0:3b683da943e6	1534	positionControl_Mul_ensemble_complex(4,V_m,servo131, position131);
Shanglin	0:3b683da943e6	1535	verifacation();
Shanglin	0:3b683da943e6	1536	positionControl_Mul_ensemble_complex(4,V_h,servo132, position132);
Shanglin	0:3b683da943e6	1537	verifacation();
Shanglin	0:3b683da943e6	1538	wait(0.1);
Shanglin	0:3b683da943e6	1539	Pompe = 0;
Shanglin	0:3b683da943e6	1540	wait(0.2);
Shanglin	0:3b683da943e6	1541
Shanglin	0:3b683da943e6	1542	positionControl_Mul_ensemble_complex(4,V_b,servo133, position133);
Shanglin	0:3b683da943e6	1543	verifacation();
Shanglin	0:3b683da943e6	1544	positionControl_Mul_ensemble_complex(4,V_b,servo134, position134);
Shanglin	0:3b683da943e6	1545	verifacation();
Shanglin	0:3b683da943e6	1546
Shanglin	0:3b683da943e6	1547	break;
Shanglin	0:3b683da943e6	1548
Shanglin	0:3b683da943e6	1549	/*case 14:
Shanglin	0:3b683da943e6	1550	uint8_t servo141[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1551	uint8_t servo142[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1552	uint8_t servo143[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1553	uint8_t servo144[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1554
Shanglin	0:3b683da943e6	1555	uint16_t position141[4] = {504, 593, 845, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1556	uint16_t position142[4] = {609, 525, 816, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1557	uint16_t position143[4] = {420, 661, 845, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1558	uint16_t position144[4] = {418, 885, 789, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1559
Shanglin	0:3b683da943e6	1560	positionControl_Mul_ensemble_complex(4,V_m,servo141, position141);
Shanglin	0:3b683da943e6	1561	verifacation();
Shanglin	0:3b683da943e6	1562	positionControl_Mul_ensemble_complex(4,V_h,servo142, position142);
Shanglin	0:3b683da943e6	1563	wait(0.5);
Shanglin	0:3b683da943e6	1564	Pompe = 0;
Shanglin	0:3b683da943e6	1565	wait(0.5);
Shanglin	0:3b683da943e6	1566	verifacation();
Shanglin	0:3b683da943e6	1567	positionControl_Mul_ensemble_complex(4,V_b,servo143, position143);
Shanglin	0:3b683da943e6	1568	verifacation();
Shanglin	0:3b683da943e6	1569	positionControl_Mul_ensemble_complex(4,V_b,servo144, position144);
Shanglin	0:3b683da943e6	1570	verifacation();
Shanglin	0:3b683da943e6	1571
Shanglin	0:3b683da943e6	1572	break;
Shanglin	0:3b683da943e6	1573
Shanglin	0:3b683da943e6	1574	case 15:
Shanglin	0:3b683da943e6	1575	uint8_t servo151[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1576	uint8_t servo152[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1577	//uint8_t servo153[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1578	uint8_t servo154[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1579	uint8_t servo155[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1580
Shanglin	0:3b683da943e6	1581	uint16_t position151[4] = {439, 688, 723, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1582	uint16_t position152[4] = {406, 691, 789, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1583	//uint16_t position153[4] = {399, 667, 767, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1584	uint16_t position154[4] = {552, 539, 801, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1585	uint16_t position155[4] = {418, 885, 789, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1586
Shanglin	0:3b683da943e6	1587	//positionControl_Mul_ensemble_complex(4,V_m,servo151, position151);
Shanglin	0:3b683da943e6	1588	//verifacation();
Shanglin	0:3b683da943e6	1589	positionControl_Mul_ensemble_complex(4,V_h,servo152, position152);
Shanglin	0:3b683da943e6	1590	verifacation();
Shanglin	0:3b683da943e6	1591	//positionControl_Mul_ensemble_complex(4,V_b,servo153, position153);
Shanglin	0:3b683da943e6	1592	//verifacation();
Shanglin	0:3b683da943e6	1593	positionControl_Mul_ensemble_complex(4,V_b,servo154, position154);
Shanglin	0:3b683da943e6	1594	verifacation();
Shanglin	0:3b683da943e6	1595	wait(0.5);
Shanglin	0:3b683da943e6	1596	Pompe = 0;
Shanglin	0:3b683da943e6	1597	wait(0.5);
Shanglin	0:3b683da943e6	1598	positionControl_Mul_ensemble_complex(4,V_b,servo155, position155);
Shanglin	0:3b683da943e6	1599	verifacation();
Shanglin	0:3b683da943e6	1600
Shanglin	0:3b683da943e6	1601
Shanglin	0:3b683da943e6	1602	break;*/
Shanglin	0:3b683da943e6	1603
Shanglin	0:3b683da943e6	1604	case 22:
Shanglin	0:3b683da943e6	1605	uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1606	uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1607	uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1608	uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1609
Shanglin	0:3b683da943e6	1610	uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1611	uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1612	uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1613	uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1614
Shanglin	0:3b683da943e6	1615	positionControl_Mul_ensemble_complex(4,V_m,servo221, position221);
Shanglin	0:3b683da943e6	1616	verifacation();
Shanglin	0:3b683da943e6	1617	positionControl_Mul_ensemble_complex(4,V_h,servo222, position222);
Shanglin	0:3b683da943e6	1618	verifacation();
Shanglin	0:3b683da943e6	1619	positionControl_Mul_ensemble_complex(4,V_b,servo223, position223);
Shanglin	0:3b683da943e6	1620	verifacation();
Shanglin	0:3b683da943e6	1621	wait(0.1);
Shanglin	0:3b683da943e6	1622	Pompe = 0;
Shanglin	0:3b683da943e6	1623	wait(0.2);
Shanglin	0:3b683da943e6	1624	positionControl_Mul_ensemble_complex(4,V_b,servo224, position224);
Shanglin	0:3b683da943e6	1625	verifacation();
Shanglin	0:3b683da943e6	1626
Shanglin	0:3b683da943e6	1627	break;
Shanglin	0:3b683da943e6	1628
Shanglin	0:3b683da943e6	1629	case 23:
Shanglin	0:3b683da943e6	1630	uint8_t servo231[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1631	uint8_t servo232[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1632	uint8_t servo233[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1633	uint8_t servo234[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1634
Shanglin	0:3b683da943e6	1635	uint16_t position231[4] = {395, 885, 829, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1636	uint16_t position232[4] = {540, 733, 816, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1637	uint16_t position233[4] = {384, 772, 829, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1638	uint16_t position234[4] = {384, 887, 829, Pos_Engrenage_Vers_Gauche};
Shanglin	0:3b683da943e6	1639
Shanglin	0:3b683da943e6	1640	positionControl_Mul_ensemble_complex(4,V_m,servo231, position231);
Shanglin	0:3b683da943e6	1641	verifacation();
Shanglin	0:3b683da943e6	1642	positionControl_Mul_ensemble_complex(4,V_h,servo232, position232);
Shanglin	0:3b683da943e6	1643	verifacation();
Shanglin	0:3b683da943e6	1644	wait(0.1);
Shanglin	0:3b683da943e6	1645	Pompe = 0;
Shanglin	0:3b683da943e6	1646	wait(0.2);
Shanglin	0:3b683da943e6	1647	positionControl_Mul_ensemble_complex(4,V_b,servo233, position233);
Shanglin	0:3b683da943e6	1648	verifacation();
Shanglin	0:3b683da943e6	1649	positionControl_Mul_ensemble_complex(4,V_b,servo234, position234);
Shanglin	0:3b683da943e6	1650	verifacation();
Shanglin	0:3b683da943e6	1651
Shanglin	0:3b683da943e6	1652	break;
Shanglin	0:3b683da943e6	1653
Shanglin	0:3b683da943e6	1654	case 32:
Shanglin	0:3b683da943e6	1655	uint8_t servo321[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1656	uint8_t servo322[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1657	uint8_t servo323[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1658	uint8_t servo324[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1659
Shanglin	0:3b683da943e6	1660	uint16_t position321[4] = {452, 801, 849, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1661	uint16_t position322[4] = {535, 768, 805, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1662	uint16_t position323[4] = {577, 819, 699, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1663	uint16_t position324[4] = {395, 864, 832, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1664
Shanglin	0:3b683da943e6	1665	positionControl_Mul_ensemble_complex(4,V_m,servo321, position321);
Shanglin	0:3b683da943e6	1666	verifacation();
Shanglin	0:3b683da943e6	1667	positionControl_Mul_ensemble_complex(4,V_h,servo322, position322);
Shanglin	0:3b683da943e6	1668	verifacation();
Shanglin	0:3b683da943e6	1669	positionControl_Mul_ensemble_complex(4,V_b,servo323, position323);
Shanglin	0:3b683da943e6	1670	verifacation();
Shanglin	0:3b683da943e6	1671	wait(0.1);
Shanglin	0:3b683da943e6	1672	Pompe = 0;
Shanglin	0:3b683da943e6	1673	wait(0.2);
Shanglin	0:3b683da943e6	1674	positionControl_Mul_ensemble_complex(4,V_b,servo324, position324);
Shanglin	0:3b683da943e6	1675	verifacation();
Shanglin	0:3b683da943e6	1676
Shanglin	0:3b683da943e6	1677	break;
Shanglin	0:3b683da943e6	1678
Shanglin	0:3b683da943e6	1679	case 33:
Shanglin	0:3b683da943e6	1680	uint8_t servo331[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1681	uint8_t servo332[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1682	uint8_t servo333[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1683	uint8_t servo334[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1684
Shanglin	0:3b683da943e6	1685	uint16_t position331[4] = {395, 885, 829, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1686	uint16_t position332[4] = {540, 733, 816, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1687	uint16_t position333[4] = {384, 772, 829, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1688	uint16_t position334[4] = {384, 887, 829, Pos_Engrenage_centre};
Shanglin	0:3b683da943e6	1689
Shanglin	0:3b683da943e6	1690	positionControl_Mul_ensemble_complex(4,V_m,servo331, position331);
Shanglin	0:3b683da943e6	1691	verifacation();
Shanglin	0:3b683da943e6	1692	positionControl_Mul_ensemble_complex(4,V_h,servo332, position332);
Shanglin	0:3b683da943e6	1693	verifacation();
Shanglin	0:3b683da943e6	1694	wait(0.1);
Shanglin	0:3b683da943e6	1695	Pompe = 0;
Shanglin	0:3b683da943e6	1696	wait(0.2);
Shanglin	0:3b683da943e6	1697	positionControl_Mul_ensemble_complex(4,V_b,servo333, position333);
Shanglin	0:3b683da943e6	1698	verifacation();
Shanglin	0:3b683da943e6	1699	positionControl_Mul_ensemble_complex(4,V_b,servo334, position334);
Shanglin	0:3b683da943e6	1700	verifacation();
Shanglin	0:3b683da943e6	1701
Shanglin	0:3b683da943e6	1702	break;
Shanglin	0:3b683da943e6	1703
Shanglin	0:3b683da943e6	1704	case 42:
Shanglin	0:3b683da943e6	1705	uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1706	uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1707	uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1708	uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1709
Shanglin	0:3b683da943e6	1710	uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1711	uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1712	uint16_t position423[4] = {573, 818, 725, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1713	uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1714
Shanglin	0:3b683da943e6	1715	positionControl_Mul_ensemble_complex(4,V_m,servo421, position421);
Shanglin	0:3b683da943e6	1716	verifacation();
Shanglin	0:3b683da943e6	1717	positionControl_Mul_ensemble_complex(4,V_h,servo422, position422);
Shanglin	0:3b683da943e6	1718	verifacation();
Shanglin	0:3b683da943e6	1719	positionControl_Mul_ensemble_complex(4,V_b,servo423, position423);
Shanglin	0:3b683da943e6	1720	verifacation();
Shanglin	0:3b683da943e6	1721	wait(0.1);
Shanglin	0:3b683da943e6	1722	Pompe = 0;
Shanglin	0:3b683da943e6	1723	wait(0.2);
Shanglin	0:3b683da943e6	1724	positionControl_Mul_ensemble_complex(4,V_b,servo424, position424);
Shanglin	0:3b683da943e6	1725	verifacation();
Shanglin	0:3b683da943e6	1726
Shanglin	0:3b683da943e6	1727	break;
Shanglin	0:3b683da943e6	1728
Shanglin	0:3b683da943e6	1729	case 43:
Shanglin	0:3b683da943e6	1730	uint8_t servo431[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
Shanglin	0:3b683da943e6	1731	uint8_t servo432[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1732	uint8_t servo433[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1733	uint8_t servo434[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
Shanglin	0:3b683da943e6	1734
Shanglin	0:3b683da943e6	1735	uint16_t position431[4] = {395, 885, 829, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1736	uint16_t position432[4] = {540, 733, 816, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1737	uint16_t position433[4] = {384, 772, 829, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1738	uint16_t position434[4] = {384, 887, 829, Pos_Engrenage_Vers_Droit};
Shanglin	0:3b683da943e6	1739
Shanglin	0:3b683da943e6	1740	positionControl_Mul_ensemble_complex(4,V_m,servo431, position431);
Shanglin	0:3b683da943e6	1741	verifacation();
Shanglin	0:3b683da943e6	1742	positionControl_Mul_ensemble_complex(4,V_h,servo432, position432);
Shanglin	0:3b683da943e6	1743	verifacation();
Shanglin	0:3b683da943e6	1744	wait(0.1);
Shanglin	0:3b683da943e6	1745	Pompe = 0;
Shanglin	0:3b683da943e6	1746	wait(0.2);
Shanglin	0:3b683da943e6	1747
Shanglin	0:3b683da943e6	1748	positionControl_Mul_ensemble_complex(4,V_b,servo433, position433);
Shanglin	0:3b683da943e6	1749	verifacation();
Shanglin	0:3b683da943e6	1750	positionControl_Mul_ensemble_complex(4,V_b,servo434, position434);
Shanglin	0:3b683da943e6	1751	verifacation();
Shanglin	0:3b683da943e6	1752
Shanglin	0:3b683da943e6	1753	break;
Shanglin	0:3b683da943e6	1754
Shanglin	0:3b683da943e6	1755
Shanglin	0:3b683da943e6	1756	}
Shanglin	0:3b683da943e6	1757	}
Shanglin	0:3b683da943e6	1758
Shanglin	0:3b683da943e6	1759	void GetPos_Engrenage(int8_t ID)
Shanglin	0:3b683da943e6	1760	{
Shanglin	0:3b683da943e6	1761
Shanglin	0:3b683da943e6	1762	Pos_Engrenage_centre = getPos(ID);
Shanglin	0:3b683da943e6	1763	Pos_Engrenage_gauche = Pos_Engrenage_centre + 256;
Shanglin	0:3b683da943e6	1764	Pos_Engrenage_droit = Pos_Engrenage_centre - 243;
Shanglin	0:3b683da943e6	1765	Pos_Engrenage_Vers_Gauche = Pos_Engrenage_centre + 256;
Shanglin	0:3b683da943e6	1766	Pos_Engrenage_Vers_Droit = Pos_Engrenage_centre - 289;
Shanglin	0:3b683da943e6	1767	pc.printf("Posi_central=%d Posi_gauche=%d Posi_droit=%d\n",Pos_Engrenage_centre,Pos_Engrenage_gauche,Pos_Engrenage_droit);
Shanglin	0:3b683da943e6	1768
Shanglin	0:3b683da943e6	1769	}
Shanglin	0:3b683da943e6	1770
Shanglin	0:3b683da943e6	1771	void Pompe_init(PinName Pompe_pin)
Shanglin	0:3b683da943e6	1772	{
Shanglin	0:3b683da943e6	1773	PwmOut Pompe1(Pompe_pin);
Shanglin	0:3b683da943e6	1774	Pompe1.period(1);
Shanglin	0:3b683da943e6	1775	Pompe1 = 0;
Shanglin	0:3b683da943e6	1776	wait(0.1);
Shanglin	0:3b683da943e6	1777	}
Shanglin	0:3b683da943e6	1778
Shanglin	0:3b683da943e6	1779	void Pompe_essai(PinName Pompe_pin){
Shanglin	0:3b683da943e6	1780	PwmOut Pompe1(Pompe_pin);
Shanglin	0:3b683da943e6	1781	Pompe1 = 1;
Shanglin	0:3b683da943e6	1782	wait(1);
Shanglin	0:3b683da943e6	1783	Pompe1 = 0;
Shanglin	0:3b683da943e6	1784	wait(1);
Shanglin	0:3b683da943e6	1785	}
Shanglin	0:3b683da943e6	1786
Shanglin	0:3b683da943e6	1787	void Arreter_couple(int8_t ID){
Shanglin	0:3b683da943e6	1788	setTorque(ID,TORQUE_FREE);
Shanglin	0:3b683da943e6	1789	}
Shanglin	0:3b683da943e6	1790
Shanglin	0:3b683da943e6	1791	void cubes_systeme(unsigned char Cb1,unsigned char Cb2,unsigned char Cb3, int8_t ID, int8_t ID2, int8_t ID3, int8_t ID4,PinName Pompe_pin){
Shanglin	0:3b683da943e6	1792	//-----------------traitement des infos recues par CAN---------------------------------------
Shanglin	0:3b683da943e6	1793	/*unsigned char C1 = combinaison_CAN/100;
Shanglin	0:3b683da943e6	1794	unsigned char C2 = (combinaison_CAN%100)/10;
Shanglin	0:3b683da943e6	1795	unsigned char C3 = combinaison_CAN%10;
Shanglin	0:3b683da943e6	1796	pc.printf("%d %d %d",C1,C2,C3);*/
Shanglin	0:3b683da943e6	1797	unsigned short combinaison_CAN = Cb1100+Cb210+Cb3;
Shanglin	0:3b683da943e6	1798	//-------------------------------------------------------------------------------------------
Shanglin	0:3b683da943e6	1799	if((combinaison_CAN == 123) \|\| (combinaison_CAN == 321)) { //---
Shanglin	0:3b683da943e6	1800	deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1801	deplacement_cubes(2, 43,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1802	deplacement_cubes(3, 12,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1803	} else if((combinaison_CAN == 154) \|\| (combinaison_CAN == 451)) { //----
Shanglin	0:3b683da943e6	1804	deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1805	deplacement_cubes(3, 43,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1806	deplacement_cubes(5, 22,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1807	} else if((combinaison_CAN == 241) \|\| (combinaison_CAN == 142)) { //----
Shanglin	0:3b683da943e6	1808	deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1809	deplacement_cubes(5, 43,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1810	deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1811	} else if((combinaison_CAN == 253) \|\| (combinaison_CAN == 352)) { //----
Shanglin	0:3b683da943e6	1812	deplacement_cubes(1, 22,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1813	deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1814	deplacement_cubes(5, 23,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1815	} else if((combinaison_CAN == 345) \|\| (combinaison_CAN == 543)) { //----
Shanglin	0:3b683da943e6	1816	deplacement_cubes(3, 22,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1817	deplacement_cubes(1, 23,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1818	deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1819	} else if((combinaison_CAN == 314) \|\| (combinaison_CAN == 413)) { //----
Shanglin	0:3b683da943e6	1820	deplacement_cubes(4, 22,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1821	deplacement_cubes(3, 23,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1822	deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1823	} else if((combinaison_CAN == 425) \|\| (combinaison_CAN == 524)) { //----
Shanglin	0:3b683da943e6	1824	deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1825	deplacement_cubes(3, 13,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1826	deplacement_cubes(2, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1827	} else if((combinaison_CAN == 432) \|\| (combinaison_CAN == 234)) { //----
Shanglin	0:3b683da943e6	1828	deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1829	deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1830	deplacement_cubes(5, 33,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1831	} else if((combinaison_CAN == 531) \|\| (combinaison_CAN == 135)) { //----
Shanglin	0:3b683da943e6	1832	deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1833	deplacement_cubes(4, 13,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1834	deplacement_cubes(5, 32,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1835	} else if((combinaison_CAN == 512) \|\| (combinaison_CAN == 215)) { //----
Shanglin	0:3b683da943e6	1836	deplacement_cubes(4, 12,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1837	deplacement_cubes(5, 13,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1838	deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin);
Shanglin	0:3b683da943e6	1839	}
Shanglin	0:3b683da943e6	1840	}
Shanglin	0:3b683da943e6	1841
Shanglin	0:3b683da943e6	1842	void monter_immeble(unsigned char nb_bras, unsigned char C1,unsigned char C2,unsigned char C3){
Shanglin	0:3b683da943e6	1843	cubes_systeme(C1,C2,C3,ID_1,ID_2,ID_3,ID_4,Pompe_pin1);
Shanglin	0:3b683da943e6	1844	/*if(nb_bras == 2){
Shanglin	0:3b683da943e6	1845	cubes_systeme(C1,C2,C3,ID,ID2,ID3,ID4,Pompe_pin1); //deuximeme bras
Shanglin	0:3b683da943e6	1846	}*/
Shanglin	0:3b683da943e6	1847	}
Shanglin	0:3b683da943e6	1848
Shanglin	0:3b683da943e6	1849	void Systeme_deplacement_cube_init(void){
Shanglin	0:3b683da943e6	1850
Shanglin	0:3b683da943e6	1851	Bras_Droite_init();
Shanglin	0:3b683da943e6	1852	}
Shanglin	0:3b683da943e6	1853
Shanglin	0:3b683da943e6	1854	void Bras_Droite_init(void){
Shanglin	0:3b683da943e6	1855	Interrupt4_en();
Shanglin	0:3b683da943e6	1856	wait_ms(0.5);
Shanglin	0:3b683da943e6	1857	clear(ID_1);
Shanglin	0:3b683da943e6	1858	clear(ID_2);
Shanglin	0:3b683da943e6	1859	clear(ID_2);
Shanglin	0:3b683da943e6	1860	clear(ID_4);
Shanglin	0:3b683da943e6	1861	setTorque(ID_1,TORQUE_ON);
Shanglin	0:3b683da943e6	1862	setTorque(ID_2,TORQUE_ON);
Shanglin	0:3b683da943e6	1863	setTorque(ID_3,TORQUE_ON);
Shanglin	0:3b683da943e6	1864	setTorque(ID_4,TORQUE_ON);
Shanglin	0:3b683da943e6	1865	wait_ms(0.3);
Shanglin	0:3b683da943e6	1866	Pompe_init(Pompe_pin1);
Shanglin	0:3b683da943e6	1867	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	12 Apr 2018
Imports:	2
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	96

lib.cpp@0:3b683da943e6, 2018-04-12 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning