CRAC Team
CAN_Servomoteur_V1_Haoxuan avec lib de servo
Dependencies: mbed
lib.cpp
- Committer:
- Shanglin
- Date:
- 2018-04-12
- Revision:
- 0:3b683da943e6
File content as of revision 0:3b683da943e6:
#include "mbed.h"
#include "lib.h"
//RawSerial pc(SERIAL_TX, SERIAL_RX, 57600);
//RawSerial serial4(PB_6,PB_7,57600);
#define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
#define tolerance_en_position_negatif -16
#define V_b 45
#define V_m 45
#define V_h 45
#define TEMPO_R 30
//------------------------------------------a choisir---------------------------
#define nombre_servomoteur 3 //
//
#define ID_1 0xFD //=253 premiere //
#define ID_3 0x0D //=13 troixieme //
#define ID_2 0x0C //=12 deuxieme //
#define ID_4 0x09 //Avec engrenage
#define Pompe_pin1 PA_15 //Pin de Pompe
//------------------------------------------------------------------------------
uint16_t Pos_Engrenage_centre = 351;
uint16_t Pos_Engrenage_gauche = 607;
uint16_t Pos_Engrenage_droit = 108;
uint16_t Pos_Engrenage_Vers_Droit = 62;
uint16_t Pos_Engrenage_Vers_Gauche = 630;
//----------------------------variables de reception----------------------------
uint8_t rx[300];
uint8_t rx2[256];
unsigned char size_reponse=100;
unsigned char recevoir = 0;
unsigned char i2 = 0;
unsigned char flag_serial4_receive2 = 0;
//--------------------variables et fonction de verification---------------------
int16_t pos_position = 0, get_pos = 0, pos_ID = 0;
uint8_t pos_led = 0, Status = 0,iID = 0;
uint8_t nombre_servo = 0;
uint8_t pos_time = 0;
uint16_t position_servo_mul[20];
uint8_t data_servo_mul[40];
uint8_t flag_correction = 0;
float new_tempo=0;
float tab_tempo[20];
uint16_t position_servo_mul_different[20];
uint8_t data_servo_mul_different[60];
int8_t my_Tor = 0;
int8_t Tension_inter = 0;
float Tension = 0;
uint8_t coeffient_time = 1;
uint8_t veri = 0;
typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ;
static type_etat etat=pos;
void verifacation()
{
uint8_t i = 0;
switch(etat) {
case pos:
//------------------------Status--------------------
Status = getStatus(pos_ID);
wait_ms(3);
pc.printf("status = %d",Status);
switch(Status) {
case 0:
break;
case 2: //Exceed allowed POT limit
pc.printf("ERR-Depasse la limite de position\n");
//clean_ERR(pos_ID);
//wait_ms(500);
clear(pos_ID);
//positionControl(pos_ID, 1000, 3, GLED_ON);
wait_ms(3);
Status = getStatus(pos_ID);
wait_ms(3);
pc.printf("status = %d",Status);
break;
}
//------------------Torque et position------------------------------
my_Tor = Get_Torque(pos_ID);
wait_ms(5);
//pc.printf("my_Tor = %x\n",my_Tor);
while(my_Tor != 0x60) {
setTorque(pos_ID,TORQUE_ON);
my_Tor = Get_Torque(pos_ID);
wait_ms(5);
}
Tension_inter = Get_Tension_actuelle(pos_ID);
Tension = Tension_inter*0.074;
if(Tension <=6.60) {
coeffient_time = 6;
} else if(Tension <= 6.90) {
coeffient_time = 4;
} else if(Tension <= 7.10) {
coeffient_time = 2;
} else if(Tension > 7.10) {
coeffient_time = 1;
}
get_pos = getPos(pos_ID);
pc.printf("P4=%d ",get_pos);
if(((get_pos - pos_position)>tolerance_en_position)||((get_pos - pos_position)<tolerance_en_position_negatif)) {
if((get_pos - pos_position)>tolerance_en_position) {
new_tempo=(get_pos - pos_position)*0.084*coeffient_time + 1;
if (new_tempo > 254) new_tempo = 254;
} else if((get_pos - pos_position)<tolerance_en_position_negatif) {
new_tempo=(get_pos - pos_position)*0.084*coeffient_time +1;
if (new_tempo > 254) new_tempo = 254;
}
positionControl(pos_ID, pos_position, new_tempo, pos_led);
pc.printf("Correction!\n");
}
break;
case pos_mul_complex:
//---------------------------Status---------------------------
for(i=0; i<nombre_servo; i++) {
Status = getStatus(data_servo_mul[1+2*i]);
pc.printf("status = %d",Status);
switch(Status) {
case 0:
break;
case 2: //Exceed allowed POT limit
//pc.printf("ERR-Depasse la limite de position\n");
//clean_ERR(id);
//wait_ms(500);
clear(data_servo_mul[1+2*i]);
//positionControl(id, 1000, 3, GLED_ON);
//wait_ms(3);
//Status = getStatus(data_servo_mul[1+2*i]);
//wait_ms(3);
//pc.printf("status = %d",Status);
break;
}
}
//----------------------Torque et position--------------------------
for(i=0; i<nombre_servo; i++) {
my_Tor = Get_Torque(data_servo_mul[1+2*i]);
while(my_Tor != 0x60) {
setTorque(data_servo_mul[1+2*i],TORQUE_ON);
my_Tor = Get_Torque(data_servo_mul[1+2*i]);
//pc.printf(" SET_TORQUE ");
Status = getStatus(data_servo_mul[1+2*i]);
clear(data_servo_mul[1+2*i]);
Status = getStatus(data_servo_mul[1+2*i]);
}
}
veri = 0;
while(veri < nombre_servo){
for(i=0; i<nombre_servo; i++) {
my_Tor = Get_Torque(data_servo_mul[1+2*i]);
while(my_Tor != 0x60) {
setTorque(data_servo_mul[1+2*i],TORQUE_ON);
my_Tor = Get_Torque(data_servo_mul[1+2*i]);
//pc.printf(" SET_TORQUE ");
Status = getStatus(data_servo_mul[1+2*i]);
clear(data_servo_mul[1+2*i]);
Status = getStatus(data_servo_mul[1+2*i]);
}
}
for(i=0; i<nombre_servo; i++) {
Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i]);
Tension = Tension_inter*0.074;
if(Tension <=6.60) {
coeffient_time = 6;
} else if(Tension <= 6.90) {
coeffient_time = 4;
} else if(Tension <= 7.10) {
coeffient_time = 2;
} else if(Tension > 7.10) {
coeffient_time = 1;
}
get_pos = getPos(data_servo_mul[1+2*i]);
pc.printf("PosiM=%d ",get_pos);
if((get_pos - position_servo_mul[i])>tolerance_en_position) {
tab_tempo[i]=(get_pos - position_servo_mul[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
if (tab_tempo[i] > 254) tab_tempo[i] = 254;
flag_correction = 1;
} else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1;
if (tab_tempo[i] > 254) tab_tempo[i] = 254;
flag_correction = 1;
}
}
if(flag_correction == 1) {
new_tempo = 0;
for(i=0; i<nombre_servo; i++) {
if(tab_tempo[i]>new_tempo) {
new_tempo = tab_tempo[i];
}
}
flag_correction = 0;
positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul);
pc.printf("Correction!\n");
}
veri = 0;
for(i=0; i<nombre_servo; i++) {
get_pos = getPos(data_servo_mul[1+2*i]);
pc.printf("PosiM=%d ",get_pos);
if((get_pos - position_servo_mul[i])>tolerance_en_position) {
tab_tempo[i]=(get_pos - position_servo_mul[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
if (tab_tempo[i] > 254) tab_tempo[i] = 254;
flag_correction = 1;
} else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1;
if (tab_tempo[i] > 254) tab_tempo[i] = 254;
flag_correction = 1;
}else //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif))
{
veri++;
}
}
}
break;
case pos_mul_complex_different:
//---------------------------Status---------------------------
for(i=0; i<nombre_servo; i++) {
Status = getStatus(data_servo_mul_different[1+3*i]);
//pc.printf("status = %d",Status);
switch(Status) {
case 0:
break;
case 2: //Exceed allowed POT limit
//pc.printf("ERR-Depasse la limite de position\n");
//clean_ERR(id);
//wait_ms(500);
clear(data_servo_mul_different[1+3*i]);
//positionControl(id, 1000, 3, GLED_ON);
//wait_ms(3);
//Status = getStatus(data_servo_mul_different[1+2*i]);
//wait_ms(3);
//pc.printf("status = %d",Status);
break;
}
}
//-------------------Torque et position-----------------------------
for(i=0; i<nombre_servo; i++) {
my_Tor = Get_Torque(data_servo_mul_different[1+3*i]);
while(my_Tor != 0x60) {
setTorque(data_servo_mul_different[1+3*i],TORQUE_ON);
my_Tor = Get_Torque(data_servo_mul_different[1+3*i]);
//wait_ms(5);
//pc.printf(" SET_TORQUE ");
}
}
for(i=0; i<nombre_servo; i++) {
Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i]);
Tension = Tension_inter*0.074;
if(Tension <=6.60) {
coeffient_time = 6;
} else if(Tension <= 6.90) {
coeffient_time = 4;
} else if(Tension <= 7.10) {
coeffient_time = 2;
} else if(Tension > 7.10) {
coeffient_time = 1;
}
get_pos = getPos(data_servo_mul_different[1+3*i]);
pc.printf("PosiM=%d ",get_pos);
if((get_pos - position_servo_mul_different[i])>tolerance_en_position) {
tab_tempo[i]=(get_pos - position_servo_mul_different[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084
if (tab_tempo[i] > 254) tab_tempo[i] = 254;
data_servo_mul_different[2+3*i] = tab_tempo[i];
flag_correction = 1;
} else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) {
tab_tempo[i]=(position_servo_mul_different[i] - get_pos)*0.084*coeffient_time+1;
if (tab_tempo[i] > 254) tab_tempo[i] = 254;
data_servo_mul_different[2+3*i] = tab_tempo[i];
flag_correction = 1;
}
}
if(flag_correction == 1) {
flag_correction = 0;
positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different);
pc.printf("Correction!\n");
}
break;
}
}
//---------------------fonction d'interruption de reception---------------------
unsigned char flag_perdu_info = 0, indicateur = 0, Size_trame = 0, old_valueserial4 = 0;
unsigned char char_receive_pc[100];
unsigned char char_receive_serial4[100];
unsigned char valueserial4=0;
unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial4_receive=0;
void receive_serial4()
{
char_receive_serial4[valueserial4]=serial4.getc();
automate();
}
void Interrupt4_en(void){
serial4.attach(&receive_serial4,Serial::RxIrq);
}
void automate()
{
typedef enum {Attente,FF,Size,Data} type_etat;
static type_etat etat=Attente;
//pc.printf("coucou");
//pc.printf("%d\r\n", char_receive_serial4[valueserial4]);
switch (etat) {
case Attente:
if(char_receive_serial4[0] == 0xFF)
{
etat = FF;
valueserial4 = 1;
}
break;
case FF:
if(char_receive_serial4[1] == 0xFF)
{
etat = Size;
valueserial4 = 2;
}
else
{
etat = Attente;
valueserial4 = 0;
flag_perdu_info = 1; //flag_perdu_info
}
break;
case Size:
if(char_receive_serial4[2] < 7)
{
etat = Attente;
valueserial4 = 0;
flag_perdu_info = 1; //flag_perdu_info
} else {
etat = Data;
old_valueserial4 = 2;
valueserial4 = 3;
}
Size_trame = char_receive_serial4[2];
break;
case Data:
if((valueserial4-2)==(Size_trame-3))
{
flag_serial4_receive = 1;
etat = Attente;
valueserial4 = 0;
} else {
valueserial4++;
}
break;
default:break;
}
}
//----------------xxxxx----fonction de fermture de serial-----------------------
/*void N_Herkulex()
{
if(Sv != NULL)
delete Sv;
if(recevoir==2) {
size_reponse = rx2[recevoir];
}
}*/
//--------------2e fonction d'effacer les erreurs (pas utilisee)----------------
int8_t clean_ERR(uint8_t id)
{
uint8_t Cx[11]; //{0xFF,0xFF,0x0B,0xFD,0x03,0xC6,0x38,0x30,0x02,0x00,0x00};
uint8_t Cr[9]; //{0xFF,0xFF,0x09,0xFD,0x04,0xC2,0x3C,0x30,0x02};
Cx[0] = HEADER; // Packet Header (0xFF)
Cx[1] = HEADER; // Packet Header (0xFF)
Cx[2] = MIN_PACKET_SIZE + 4; // Packet Size
Cx[3] = id; // Servo ID
Cx[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
Cx[5] = 0xC6; // Checksum1
Cx[6] = 0x38; // Checksum2
Cx[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address
Cx[8] = BYTE2; // Length
Cx[9] = 0;
Cx[10]= 0;
Cr[0] = HEADER; // Packet Header (0xFF)
Cr[1] = HEADER; // Packet Header (0xFF)
Cr[2] = MIN_PACKET_SIZE + 2; // Packet Size
Cr[3] = id; // Servo ID
Cr[4] = CMD_RAM_READ; // Command Ram Read
Cr[5] = 0xC2; // Checksum1
Cr[6] = 0x3C; // Checksum2
Cr[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address
Cr[8] = BYTE2; // Length
size_reponse = 13;
for(uint8_t i = 0; i < 11 ; i++) {
while(serial4.writeable() == 0);
serial4.putc(Cx[i]);
}
wait_ms(0.3);
for(uint8_t i = 0; i < 9 ; i++) {
while(serial4.writeable() == 0);
serial4.putc(Cr[i]);
}
wait_ms(0.5);
uint8_t rxBuf[13];
if(flag_serial4_receive2) {
for(i2 = 0; i2<13; i2++) {
rxBuf[i2] = rx2[i2];
pc.printf(" %x",rx2[i2]);
}
flag_serial4_receive2=0;
}
// Checksum1
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
if (chksum1 != rxBuf[5]) {
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
return -1;
}
return 0;
}
//-------------------------fonction de transmission-----------------------------
//comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise
void txPacket(uint8_t packetSize, uint8_t* data)
{
/*#ifdef HERKULEX_DEBUG
pc->printf("[TX]");
#endif
for(uint8_t i = 0; i < packetSize ; i++)
{
#ifdef HERKULEX_DEBUG
pc->printf("%02X ",data[i]);
#endif
txd->putc(data[i]);
}
#ifdef HERKULEX_DEBUG
pc->printf("\n");
#endif*/
for(uint8_t i = 0; i < packetSize ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(data[i]);
wait_us(100);
}
wait_ms(TEMPO_R);
}
//----------------------------fonction de reception-----------------------------
//comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise
void rxPacket(uint8_t packetSize, uint8_t* data)
{
/*#ifdef HERKULEX_DEBUG
pc->printf("[RX]");
#endif
for (uint8_t i=0; i < packetSize; i++)
{
data[i] = rxd->getc();
#ifdef HERKULEX_DEBUG
pc->printf("%02X ",data[i]);
#endif
}
#ifdef HERKULEX_DEBUG
pc->printf("\n");
#endif*/
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
data[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
}
//----------------------fonction d'effacer les erreurs--------------------------
void clear(uint8_t id)
{
uint8_t txBuf[11];
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_STATUS_ERROR; // Address
txBuf[8] = BYTE2; // Length
txBuf[9] = 0; // Clear RAM_STATUS_ERROR
txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
for(uint8_t i = 0; i < 11 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
wait_ms(TEMPO_R);
}
//----------------fonction de mis a jour le couple de servo---------------------
void setTorque(uint8_t id, uint8_t cmdTorue)
{
uint8_t txBuf[10];
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_TORQUE_CONTROL; // Address
txBuf[8] = BYTE1; // Length
txBuf[9] = cmdTorue; // Torque ON
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
for(uint8_t i = 0; i < 10 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
wait_ms(TEMPO_R);
}
//-------------fonction de controle de position pour un seul servo--------------
void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
{
float tempo=0;
//if (position > 1023) return; //1002-21
if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
tempo=playtime*0.012;
pos_ID = id;
uint8_t txBuf[12];
etat = pos;
pos_position = position;
pos_time = playtime;
pos_led = setLED;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = playtime; // Playtime
txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
txBuf[11] = id; // Servo ID
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
//txPacket(12, txBuf);
for(uint8_t i = 0; i < 12 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
wait(tempo);
wait_ms(TEMPO_R);
}
//-------------fonction de controle de vitesse pour un seul servo---------------
//Comme je n'ai pas utilise cette fonction, je ne l'ai pas encore change
void velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
{
if (speed > 1023 || speed < -1023) return;
uint8_t txBuf[12];
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = 0; // Playtime, unmeaningful in turn mode
txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off
txBuf[11] = id; // Servo ID
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
txPacket(12, txBuf);
wait_ms(TEMPO_R);
}
//--------------------fonction d'acquis d'etat d'un servo-----------------------
int8_t getStatus(uint8_t id)
{
uint8_t status;
uint8_t txBuf[7];
size_reponse = 9;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_STAT; // Status Error, Status Detail request
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
for(uint8_t i = 0; i < 7 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
// send packet (mbed -> herkulex)
uint8_t rxBuf[9];
wait_ms(TEMPO_R);
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
rxBuf[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
// Checksum1
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
if (chksum1 != rxBuf[5]) {
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
return -1;
}
status = rxBuf[7]; // Status Error
//status = rxBuf[8]; // Status Detail
return status;
}
//------------------fonction de lire la position actuelle-----------------------
int16_t getPos(uint8_t id)
{
uint16_t position = 0;
uint8_t txBuf[9];
size_reponse = 13;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_READ; // Command Ram Read
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_CALIBRATED_POSITION; // Address
txBuf[8] = BYTE2;
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
for(uint8_t i = 0; i < 9 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
// send packet (mbed -> herkulex)
uint8_t rxBuf[13];
wait_ms(TEMPO_R);
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
rxBuf[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
// Checksum1
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
if (chksum1 != rxBuf[5]) {
/*#ifdef HERKULEX_DEBUG
pc->printf("Checksum1 fault\n");
#endif*/
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
/* #ifdef HERKULEX_DEBUG
pc->printf("Checksum2 fault\n");
#endif*/
return -1;
}
position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
/* #ifdef HERKULEX_DEBUG
pc->printf("position = %04X(%d)\n", position, position);
#endif*/
//}
return position;
}
//---------------fonction d'acquis d'etat de couple d'un servo------------------
int8_t Get_Torque(int8_t id)
{
uint8_t txBuf[9];
int8_t Tor = 0;
uint8_t iv=0;
for(iv=0; iv<20; iv++) {
rx2[iv] = 0;
}
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_READ; // Command Ram Read
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_TORQUE_CONTROL;
txBuf[8] = BYTE1; // Length
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
pc.printf(" Torque ");
for(uint8_t i = 0; i < 9 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
// send packet (mbed -> herkulex)
uint8_t rxBuf[12];
//wait_ms(3);
wait_ms(TEMPO_R);
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
rxBuf[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
if (chksum1 != rxBuf[5]) {
/*#ifdef HERKULEX_DEBUG
pc->printf("Checksum1 fault\n");
#endif*/
recevoir=0;
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
/* #ifdef HERKULEX_DEBUG
pc->printf("Checksum2 fault\n");
#endif*/
recevoir=0;
return -1;
}
Tor = rxBuf[9];
/* #ifdef HERKULEX_DEBUG
pc->printf("position = %04X(%d)\n", position, position);
#endif*/
//}
return Tor;
}
//---------------fonction pour lire le temperature max pour un servo------------
int8_t Get_Temperature_MAX(int8_t id)
{
uint8_t txBuf[9];
int8_t tempeMAX = 0;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_READ; // Command Ram Read
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_MAX_TEMPERATURE;
txBuf[8] = BYTE1; // Length
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
pc.printf(" tempeMAX ");
for(uint8_t i = 0; i < 9 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
// send packet (mbed -> herkulex)
uint8_t rxBuf[12];
//wait_ms(3);
wait_ms(TEMPO_R);
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
rxBuf[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
if (chksum1 != rxBuf[5]) {
/*#ifdef HERKULEX_DEBUG
pc->printf("Checksum1 fault\n");
#endif*/
recevoir=0;
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
/* #ifdef HERKULEX_DEBUG
pc->printf("Checksum2 fault\n");
#endif*/
recevoir=0;
return -1;
}
tempeMAX = rxBuf[9];
/* #ifdef HERKULEX_DEBUG
pc->printf("position = %04X(%d)\n", position, position);
#endif*/
//}
return tempeMAX;
}
//--------fonction de controle de position pour deux servo(same playtime)-------
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)
{
float tempo=0;
//if (position > 1023) return; //1002-21
if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
tempo=playtime*0.012;
uint8_t txBuf[16];
etat = pos;
pos_position = position;
pos_time = playtime;
pos_led = setLED;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[3] = 254; // broadcast ID
txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = playtime; // Playtime
txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
txBuf[11] = id; // Servo ID
txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off
txBuf[15] = id2; // Servo ID
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
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;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
for(uint8_t i = 0; i < 16 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
wait(tempo);
wait_ms(TEMPO_R);
}
//-----fonction de controle de position pour deux servo(different playtime)----- //a changer...
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)
{
float tempo=0;
//if (position > 1023) return; //1002-21
if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
if(playtime>playtime2) {
tempo=playtime*0.012;
} else if(playtime<playtime2) {
tempo=playtime2*0.012;
}
uint8_t txBuf[17];
etat = pos;
pos_position = position;
pos_time = playtime;
pos_led = setLED;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[3] = 254; // broadcast ID
txBuf[4] = CMD_I_JOG; // Command I JOG
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
txBuf[9] = POS_MODE | setLED; // Pos Mode and LED on/off
txBuf[10] = id; // Servo ID
txBuf[11] = playtime; // Playtime
txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off
txBuf[15] = id2; // Servo ID
txBuf[16] = playtime2; // Playtime
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
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;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
//txPacket(12, txBuf);
for(uint8_t i = 0; i < 17 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
wait(tempo);
wait_ms(TEMPO_R);
}
//-----fonction de controle de position pour plusieurs servo(same playtime)-----
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
{
float tempo=0;
uint8_t taille = 0,i = 0,idata = 0, ipos = 0;
//if (position > 1023) return; //1002-21
if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
tempo=playtime*0.012;
taille = 7 + 1 + 4 * nb_servo;
nombre_servo = nb_servo;
pos_time = playtime;
uint8_t txBuf[taille];
etat = pos_mul_complex;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size
//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[3] = 254; // broadcast ID
txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = playtime; // Playtime
for(i=0; i<nb_servo; i++) {
txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
position_servo_mul[ipos] = pos[ipos];
ipos++;
txBuf[10+i*4] = POS_MODE | data[idata]; // Pos Mode and LED on/off
data_servo_mul[idata] = data[idata];
idata++;
txBuf[11+i*4] = data[idata]; // Servo ID
data_servo_mul[idata] = data[idata];
idata++;
}
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7];
for(i=1; i<(taille-7); i++) {
txBuf[5]=txBuf[5]^txBuf[7+i];
}
txBuf[5] = txBuf[5]& 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
for(uint8_t i = 0; i < taille ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
//wait(tempo);
//wait_ms(TEMPO_R);
}
//--fonction de controle de position pour plusieurs servo(different playtime)---
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
{
float tempo=0;
uint8_t Max_playtime = 0;
uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0;
//if (position > 1023) return; //1002-21
//if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
for(iplay_time=0; iplay_time<nb_servo; iplay_time++) {
if(Max_playtime<data[2+3*iplay_time]) {
Max_playtime=data[2+3*iplay_time];
}
}
tempo=Max_playtime*0.012;
taille = 7 + 5 * nb_servo;
nombre_servo = nb_servo;
uint8_t txBuf[taille];
etat = pos_mul_complex_different;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size
//txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[3] = 254; // broadcast ID
txBuf[4] = CMD_I_JOG; // Command I JOG (0x06)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
for(i=0; i<nb_servo; i++) {
txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
position_servo_mul_different[ipos] = pos[ipos];
ipos++;
txBuf[9+i*5] = POS_MODE | data[idata]; // Pos Mode and LED on/off
data_servo_mul_different[idata] = data[idata];
idata++;
txBuf[10+i*5] = data[idata]; // Servo ID
data_servo_mul_different[idata] = data[idata];
idata++;
txBuf[11+i*5] = data[idata]; // Playtime
data_servo_mul_different[idata] = data[idata];
idata++;
}
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4];
for(i=1; i<(taille-6); i++) {
txBuf[5]=txBuf[5]^txBuf[6+i];
}
txBuf[5] = txBuf[5]& 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
// send packet (mbed -> herkulex)
for(uint8_t i = 0; i < taille ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
wait(tempo);
wait_ms(TEMPO_R);
}
//---------------fonction pour lire la tension min pour un servo----------------
int8_t Get_Tension_MIN(int8_t id)
{
uint8_t txBuf[9];
int8_t tensionMIN = 0;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_READ; // Command Ram Read
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_MIN_VOLTAGE;
txBuf[8] = BYTE1; // Length
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
pc.printf(" tensionMIN ");
for(uint8_t i = 0; i < 9 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
// send packet (mbed -> herkulex)
uint8_t rxBuf[12];
//wait_ms(3);
wait_ms(TEMPO_R);
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
rxBuf[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
if (chksum1 != rxBuf[5]) {
/*#ifdef HERKULEX_DEBUG
pc->printf("Checksum1 fault\n");
#endif*/
recevoir=0;
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
/* #ifdef HERKULEX_DEBUG
pc->printf("Checksum2 fault\n");
#endif*/
recevoir=0;
return -1;
}
tensionMIN = rxBuf[9];
/* #ifdef HERKULEX_DEBUG
pc->printf("position = %04X(%d)\n", position, position);
#endif*/
//}
return tensionMIN;
}
//-------------fonction pour controle la tension min pour un servo--------------
void Set_Tension_MIN(int8_t id,uint8_t Tension_Min)
{
uint8_t txBuf[10];
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_MIN_VOLTAGE;
txBuf[8] = BYTE1; // Length
txBuf[9] = Tension_Min;
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
pc.printf(" tensionMIN ");
for(uint8_t i = 0; i < 10 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
//wait_ms(3);
wait_ms(TEMPO_R);
}
//------------fonction pour lire la tension acttuelle pour un servo-------------
int8_t Get_Tension_actuelle(int8_t id)
{
uint8_t txBuf[9];
int8_t tension = 0;
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
txBuf[3] = id; // Servo ID
txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_VOLTAGE;
txBuf[8] = BYTE2; // Length
// Check Sum1 and Check Sum2
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
txBuf[6] = (~txBuf[5])&0xFE;
pc.printf(" tension ");
for(uint8_t i = 0; i < 9 ; i++)
{
while(serial4.writeable() == 0);
serial4.putc(txBuf[i]);
wait_us(100);
}
// send packet (mbed -> herkulex)
uint8_t rxBuf[13];
//wait_ms(3);
wait_ms(TEMPO_R);
if(flag_serial4_receive)
{
for(unsigned char i4=0;i4<Size_trame; i4++)
{
rxBuf[i4] = char_receive_serial4[i4];
pc.printf("%d ",(int)char_receive_serial4[i4]);
}
flag_serial4_receive=0;
valueserial4=0;
}
uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
if (chksum1 != rxBuf[5]) {
/*#ifdef HERKULEX_DEBUG
pc->printf("Checksum1 fault\n");
#endif*/
recevoir=0;
return -1;
}
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
if (chksum2 != rxBuf[6]) {
/* #ifdef HERKULEX_DEBUG
pc->printf("Checksum2 fault\n");
#endif*/
recevoir=0;
return -1;
}
tension = rxBuf[9];
/* #ifdef HERKULEX_DEBUG
pc->printf("position = %04X(%d)\n", position, position);
#endif*/
//}
return tension;
}
//-----------------deplacement des cubes---------------------
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)
{
PwmOut Pompe(Pompe_pin);
switch(poi_init) {
case 5: //verifie X
//uint8_t servo1[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
//uint8_t servo2[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
uint8_t servo3[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
uint8_t servo4[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo5[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
//uint16_t position1[3] = {512, 236, 236};
//uint16_t position2[3] = {512, 512, 788};
uint16_t position3[4] = {374, 926, 788, Pos_Engrenage_centre};
uint16_t position6[4] = {481, 950, 714, Pos_Engrenage_centre};
uint16_t position4[4] = {650, 952, 511, Pos_Engrenage_centre};
uint16_t position5[4] = {451, 944, 720, Pos_Engrenage_centre};
/*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo1, position1);
verifacation();
positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo2, position2);
verifacation();*/
positionControl_Mul_ensemble_complex(4,V_b,servo3, position3);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo3, position6);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo4, position4);
verifacation();
wait(0.3);
Pompe = 1;
wait(0.1);
positionControl_Mul_ensemble_complex(4,V_m,servo5, position5);
verifacation();
break;
case 3: //verifie X
//uint8_t servo31[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
//uint8_t servo32[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
uint8_t servo33[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
uint8_t servo34[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
uint8_t servo35[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
//uint16_t position31[3] = {512, 236, 236};
//uint16_t position32[3] = {512, 512, 788};
uint16_t position33[4] = {374, 926, 788, Pos_Engrenage_centre};
uint16_t position34[4] = {673, 837, 595, Pos_Engrenage_centre}; //647, 842, 604
uint16_t position36[4] = {547, 845, 702, Pos_Engrenage_centre};
uint16_t position35[4] = {451, 944, 720, Pos_Engrenage_centre};
//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo31, position31);
//verifacation();
//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo32, position32);
//verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo33, position33);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo34, position34);
verifacation();
wait(0.3);
Pompe = 1;
wait(0.1);
positionControl_Mul_ensemble_complex(4,V_m,servo33, position36);
verifacation();
positionControl_Mul_ensemble_complex(4,V_m,servo35, position35);
verifacation();
break;
case 2:
//uint8_t servo20[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
//uint8_t servo21[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
//uint8_t servo22[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
uint8_t servo23[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
uint8_t servo24[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo25[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
//uint16_t position20[3] = {512, 236, 236};
//uint16_t position21[3] = {512, 236, 236};
//uint16_t position22[3] = {512, 512, 788};
uint16_t position23[4] = {374, 926, 788, Pos_Engrenage_gauche};
uint16_t position24[4] = {673, 837, 595, Pos_Engrenage_gauche};
uint16_t position25[4] = {451, 944, 720, Pos_Engrenage_gauche};
/*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo20, position20);
verifacation();
wait(0.2);
positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo21, position21);
verifacation();
positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo22, position22);
verifacation();*/
positionControl_Mul_ensemble_complex(4,V_b,servo23, position23);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo24, position24);
verifacation();
wait(0.3);
Pompe = 1;
wait(0.1);
positionControl_Mul_ensemble_complex(4,V_m,servo25, position25);
verifacation();
break;
case 4:
//uint8_t servo40[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
//uint8_t servo41[6] = {GLED_ON, ID4, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
//uint8_t servo42[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
uint8_t servo43[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
uint8_t servo44[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo45[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
//uint16_t position40[3] = {512, 236, 236};
//uint16_t position41[3] = {512, 236, 236};
//uint16_t position42[3] = {512, 512, 788};
uint16_t position43[4] = {374, 926, 788, Pos_Engrenage_droit};
uint16_t position44[4] = {673, 837, 595, Pos_Engrenage_droit};
uint16_t position45[4] = {451, 944, 720, Pos_Engrenage_droit};
/*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo40, position40);
verifacation();
wait(0.2);
positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo41, position41);
verifacation();
positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo42, position42);
verifacation();*/
positionControl_Mul_ensemble_complex(4,V_b,servo43, position43);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo44, position44);
verifacation();
wait(0.3);
Pompe = 1;
wait(0.1);
positionControl_Mul_ensemble_complex(4,V_m,servo45, position45);
verifacation();
break;
case 1: //verifie X
//uint8_t servo11[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
//uint8_t servo12[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
uint8_t servo13[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
uint8_t servo14[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
uint8_t servo15[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
uint8_t servo16[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
//uint16_t position11[3] = {512, 236, 236};
//uint16_t position12[3] = {512, 512, 788};
uint16_t position13[4] = {374, 926, 788, Pos_Engrenage_centre};
uint16_t position17[4] = {658, 657, 790, Pos_Engrenage_centre};
uint16_t position14[4] = {731, 675, 695, Pos_Engrenage_centre};
uint16_t position15[4] = {658, 657, 790, Pos_Engrenage_centre};
uint16_t position16[4] = {417, 885, 796, Pos_Engrenage_centre};
//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo11, position11);
//verifacation();
//positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo12, position12);
//verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo13, position13);
verifacation();
positionControl_Mul_ensemble_complex(4,V_m,servo13, position17);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo14, position14);
verifacation();
wait(0.3);
Pompe = 1;
wait(0.1);
positionControl_Mul_ensemble_complex(4,V_h,servo15, position15);
verifacation();
//wait(2);
positionControl_Mul_ensemble_complex(4,V_h,servo16, position16);
verifacation();
break;
/*case 22:
uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_gauche};
uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_gauche};
uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_gauche};
uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_gauche};
positionControl_Mul_ensemble_complex(4,V_m,servo221, position221);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo222, position222);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo223, position223);
verifacation();
wait(0.5);
Pompe = 1;
wait(0.5);
positionControl_Mul_ensemble_complex(4,V_b,servo224, position224);
verifacation();
break;
case 42:
uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_droit};
uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_droit};
uint16_t position423[4] = {553, 819, 733, Pos_Engrenage_droit};
uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_droit};
positionControl_Mul_ensemble_complex(4,V_m,servo421, position421);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo422, position422);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo423, position423);
verifacation();
wait(0.5);
Pompe = 1;
wait(0.5);
positionControl_Mul_ensemble_complex(4,V_b,servo424, position424);
verifacation();
break;*/
}
switch(poi_fini) {
/*case 11:
uint8_t servo101[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo102[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo103[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo104[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position101[4] = {663, 615, 820, Pos_Engrenage_centre};
uint16_t position102[4] = {742, 542, 819, Pos_Engrenage_centre};
uint16_t position103[4] = {715, 660, 719, Pos_Engrenage_centre};
uint16_t position104[4] = {418, 885, 789, Pos_Engrenage_centre};
positionControl_Mul_ensemble_complex(4,V_b,servo101, position101);
verifacation();
positionControl_Mul_ensemble_complex(4,V_m,servo102, position102);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo103, position103);
verifacation();
wait(0.5);
Pompe = 0;
wait(0.5);
positionControl_Mul_ensemble_complex(4,V_b,servo104, position104);
verifacation();
break;*/
case 12:
uint8_t servo121[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo122[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo123[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo124[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position121[4] = {660, 620, 828, Pos_Engrenage_centre};
uint16_t position122[4] = {659, 669, 763, Pos_Engrenage_centre};
uint16_t position123[4] = {532, 570, 835, Pos_Engrenage_centre};
uint16_t position124[4] = {418, 885, 789, Pos_Engrenage_centre};
positionControl_Mul_ensemble_complex(4,V_m,servo121, position121);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo122, position122);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo123, position123);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo124, position124);
verifacation();
break;
case 13:
uint8_t servo131[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo132[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo133[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo134[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position131[4] = {552, 649, 853, Pos_Engrenage_centre};
uint16_t position132[4] = {647, 589, 813, Pos_Engrenage_centre};
uint16_t position133[4] = {552, 649, 853, Pos_Engrenage_centre};
uint16_t position134[4] = {418, 885, 789, Pos_Engrenage_centre};
positionControl_Mul_ensemble_complex(4,V_m,servo131, position131);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo132, position132);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo133, position133);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo134, position134);
verifacation();
break;
/*case 14:
uint8_t servo141[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo142[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo143[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo144[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position141[4] = {504, 593, 845, Pos_Engrenage_centre};
uint16_t position142[4] = {609, 525, 816, Pos_Engrenage_centre};
uint16_t position143[4] = {420, 661, 845, Pos_Engrenage_centre};
uint16_t position144[4] = {418, 885, 789, Pos_Engrenage_centre};
positionControl_Mul_ensemble_complex(4,V_m,servo141, position141);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo142, position142);
wait(0.5);
Pompe = 0;
wait(0.5);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo143, position143);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo144, position144);
verifacation();
break;
case 15:
uint8_t servo151[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo152[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
//uint8_t servo153[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo154[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo155[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position151[4] = {439, 688, 723, Pos_Engrenage_centre};
uint16_t position152[4] = {406, 691, 789, Pos_Engrenage_centre};
//uint16_t position153[4] = {399, 667, 767, Pos_Engrenage_centre};
uint16_t position154[4] = {552, 539, 801, Pos_Engrenage_centre};
uint16_t position155[4] = {418, 885, 789, Pos_Engrenage_centre};
//positionControl_Mul_ensemble_complex(4,V_m,servo151, position151);
//verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo152, position152);
verifacation();
//positionControl_Mul_ensemble_complex(4,V_b,servo153, position153);
//verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo154, position154);
verifacation();
wait(0.5);
Pompe = 0;
wait(0.5);
positionControl_Mul_ensemble_complex(4,V_b,servo155, position155);
verifacation();
break;*/
case 22:
uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_Vers_Gauche};
uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_Vers_Gauche};
uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_Vers_Gauche};
uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_Vers_Gauche};
positionControl_Mul_ensemble_complex(4,V_m,servo221, position221);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo222, position222);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo223, position223);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo224, position224);
verifacation();
break;
case 23:
uint8_t servo231[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo232[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo233[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo234[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position231[4] = {395, 885, 829, Pos_Engrenage_Vers_Gauche};
uint16_t position232[4] = {540, 733, 816, Pos_Engrenage_Vers_Gauche};
uint16_t position233[4] = {384, 772, 829, Pos_Engrenage_Vers_Gauche};
uint16_t position234[4] = {384, 887, 829, Pos_Engrenage_Vers_Gauche};
positionControl_Mul_ensemble_complex(4,V_m,servo231, position231);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo232, position232);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo233, position233);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo234, position234);
verifacation();
break;
case 32:
uint8_t servo321[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo322[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo323[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo324[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position321[4] = {452, 801, 849, Pos_Engrenage_centre};
uint16_t position322[4] = {535, 768, 805, Pos_Engrenage_centre};
uint16_t position323[4] = {577, 819, 699, Pos_Engrenage_centre};
uint16_t position324[4] = {395, 864, 832, Pos_Engrenage_centre};
positionControl_Mul_ensemble_complex(4,V_m,servo321, position321);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo322, position322);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo323, position323);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo324, position324);
verifacation();
break;
case 33:
uint8_t servo331[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo332[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo333[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo334[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position331[4] = {395, 885, 829, Pos_Engrenage_centre};
uint16_t position332[4] = {540, 733, 816, Pos_Engrenage_centre};
uint16_t position333[4] = {384, 772, 829, Pos_Engrenage_centre};
uint16_t position334[4] = {384, 887, 829, Pos_Engrenage_centre};
positionControl_Mul_ensemble_complex(4,V_m,servo331, position331);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo332, position332);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo333, position333);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo334, position334);
verifacation();
break;
case 42:
uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_Vers_Droit};
uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_Vers_Droit};
uint16_t position423[4] = {573, 818, 725, Pos_Engrenage_Vers_Droit};
uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_Vers_Droit};
positionControl_Mul_ensemble_complex(4,V_m,servo421, position421);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo422, position422);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo423, position423);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo424, position424);
verifacation();
break;
case 43:
uint8_t servo431[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
uint8_t servo432[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint8_t servo433[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
uint8_t servo434[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
uint16_t position431[4] = {395, 885, 829, Pos_Engrenage_Vers_Droit};
uint16_t position432[4] = {540, 733, 816, Pos_Engrenage_Vers_Droit};
uint16_t position433[4] = {384, 772, 829, Pos_Engrenage_Vers_Droit};
uint16_t position434[4] = {384, 887, 829, Pos_Engrenage_Vers_Droit};
positionControl_Mul_ensemble_complex(4,V_m,servo431, position431);
verifacation();
positionControl_Mul_ensemble_complex(4,V_h,servo432, position432);
verifacation();
wait(0.1);
Pompe = 0;
wait(0.2);
positionControl_Mul_ensemble_complex(4,V_b,servo433, position433);
verifacation();
positionControl_Mul_ensemble_complex(4,V_b,servo434, position434);
verifacation();
break;
}
}
void GetPos_Engrenage(int8_t ID)
{
Pos_Engrenage_centre = getPos(ID);
Pos_Engrenage_gauche = Pos_Engrenage_centre + 256;
Pos_Engrenage_droit = Pos_Engrenage_centre - 243;
Pos_Engrenage_Vers_Gauche = Pos_Engrenage_centre + 256;
Pos_Engrenage_Vers_Droit = Pos_Engrenage_centre - 289;
pc.printf("Posi_central=%d Posi_gauche=%d Posi_droit=%d\n",Pos_Engrenage_centre,Pos_Engrenage_gauche,Pos_Engrenage_droit);
}
void Pompe_init(PinName Pompe_pin)
{
PwmOut Pompe1(Pompe_pin);
Pompe1.period(1);
Pompe1 = 0;
wait(0.1);
}
void Pompe_essai(PinName Pompe_pin){
PwmOut Pompe1(Pompe_pin);
Pompe1 = 1;
wait(1);
Pompe1 = 0;
wait(1);
}
void Arreter_couple(int8_t ID){
setTorque(ID,TORQUE_FREE);
}
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){
//-----------------traitement des infos recues par CAN---------------------------------------
/*unsigned char C1 = combinaison_CAN/100;
unsigned char C2 = (combinaison_CAN%100)/10;
unsigned char C3 = combinaison_CAN%10;
pc.printf("%d %d %d",C1,C2,C3);*/
unsigned short combinaison_CAN = Cb1*100+Cb2*10+Cb3;
//-------------------------------------------------------------------------------------------
if((combinaison_CAN == 123) || (combinaison_CAN == 321)) { //---
deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(2, 43,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(3, 12,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 154) || (combinaison_CAN == 451)) { //----
deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(3, 43,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 22,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 241) || (combinaison_CAN == 142)) { //----
deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 43,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 253) || (combinaison_CAN == 352)) { //----
deplacement_cubes(1, 22,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 23,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 345) || (combinaison_CAN == 543)) { //----
deplacement_cubes(3, 22,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(1, 23,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 314) || (combinaison_CAN == 413)) { //----
deplacement_cubes(4, 22,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(3, 23,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 425) || (combinaison_CAN == 524)) { //----
deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(3, 13,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(2, 42,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 432) || (combinaison_CAN == 234)) { //----
deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 33,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 531) || (combinaison_CAN == 135)) { //----
deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(4, 13,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 32,ID,ID2,ID3,ID4,Pompe_pin);
} else if((combinaison_CAN == 512) || (combinaison_CAN == 215)) { //----
deplacement_cubes(4, 12,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(5, 13,ID,ID2,ID3,ID4,Pompe_pin);
deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin);
}
}
void monter_immeble(unsigned char nb_bras, unsigned char C1,unsigned char C2,unsigned char C3){
cubes_systeme(C1,C2,C3,ID_1,ID_2,ID_3,ID_4,Pompe_pin1);
/*if(nb_bras == 2){
cubes_systeme(C1,C2,C3,ID,ID2,ID3,ID4,Pompe_pin1); //deuximeme bras
}*/
}
void Systeme_deplacement_cube_init(void){
Bras_Droite_init();
}
void Bras_Droite_init(void){
Interrupt4_en();
wait_ms(0.5);
clear(ID_1);
clear(ID_2);
clear(ID_2);
clear(ID_4);
setTorque(ID_1,TORQUE_ON);
setTorque(ID_2,TORQUE_ON);
setTorque(ID_3,TORQUE_ON);
setTorque(ID_4,TORQUE_ON);
wait_ms(0.3);
Pompe_init(Pompe_pin1);
}