Insertion du code de l'ascenseur. TODO: Vérifier que l'ascenseur s'initialise et répond aux commandes séries 'u' et 'd'
This commit is contained in:
parent
18ff92e0e6
commit
ecad2cc7a2
@ -58,8 +58,7 @@ void setup()
|
||||
Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
|
||||
sms_sts.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
sms_sts.WheelMode(ID_Feetech);//舵机ID1切换至电机恒速模式
|
||||
//sms_sts.WriteSpe(ID_Feetech, 2400, 100);
|
||||
|
||||
cible_haute.nb_tour = 0;
|
||||
cible_haute.position = 500;
|
||||
|
||||
@ -135,53 +134,15 @@ void setup()
|
||||
actuelle.position = sms_sts.ReadPos(-1);
|
||||
}
|
||||
|
||||
Ascenseur_init();
|
||||
|
||||
while(1){
|
||||
Ascenseur_gestion();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void goto_position(int servo_id, position_t position){
|
||||
// Actualise la position
|
||||
struct position_t precedente;
|
||||
static int loin = 1;
|
||||
precedente = actuelle;
|
||||
if(sms_sts.FeedBack(servo_id)!=-1){
|
||||
actuelle.position = sms_sts.ReadPos(-1);
|
||||
}
|
||||
if(abs((int)actuelle.position - (int)precedente.position) > 2000){
|
||||
// Rebouclage compteur
|
||||
if(actuelle.position < precedente.position){
|
||||
// Sens positif
|
||||
actuelle.nb_tour++;
|
||||
}else{
|
||||
actuelle.nb_tour--;
|
||||
}
|
||||
}
|
||||
|
||||
// Sens de déplacement
|
||||
if(actuelle.nb_tour != position.nb_tour){
|
||||
if(loin != 1){
|
||||
loin = 1;
|
||||
sms_sts.WheelMode(servo_id);
|
||||
if(actuelle.nb_tour < position.nb_tour){
|
||||
sms_sts.WriteSpe(servo_id, -2400, 50);
|
||||
}else{
|
||||
sms_sts.WriteSpe(servo_id, 2400, 50);
|
||||
}
|
||||
}
|
||||
}else{
|
||||
if(loin != 0){
|
||||
loin = 0;
|
||||
sms_sts.ServoMode(servo_id);
|
||||
sms_sts.WritePosEx(ID_Feetech, position.position, 2400, 50);
|
||||
}
|
||||
}
|
||||
Serial.printf(">cible_pos:%d\n",position.position);
|
||||
Serial.printf(">cible_nb_tour:%d\n",position.nb_tour);
|
||||
Serial.printf(">act_pos:%d\n",actuelle.position);
|
||||
Serial.printf(">act_nb_tour:%d\n",actuelle.nb_tour);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
@ -189,33 +150,14 @@ void loop()
|
||||
static unsigned long myTime=0;
|
||||
static position_t cible;
|
||||
|
||||
/*
|
||||
//舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
|
||||
sms_sts.WritePosEx(1, 4095, 2400, 50);
|
||||
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
|
||||
|
||||
//舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
|
||||
sms_sts.WritePosEx(1, 0, 2400, 50);
|
||||
delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
|
||||
*/
|
||||
|
||||
Serial.print(">millis:");
|
||||
Serial.println(millis());
|
||||
if(millis() > myTime + 4000){
|
||||
myTime = millis();
|
||||
if(m_pos){
|
||||
//sms_sts.WritePosEx(ID_Feetech, 4095, 2400, 50);
|
||||
//sms_sts.WriteSpe(ID_Feetech, 2400, 50);
|
||||
cible = cible_haute;
|
||||
m_pos = 0;
|
||||
}else{
|
||||
m_pos ++;
|
||||
//sms_sts.WriteSpe(ID_Feetech, -2400, 50);
|
||||
//sms_sts.WritePosEx(ID_Feetech, 0, 2400, 50);
|
||||
//sms_sts.WriteSpe(ID_Feetech, -80, 100);
|
||||
cible = cible_basse;
|
||||
}
|
||||
|
||||
}
|
||||
goto_position(ID_Feetech, cible);
|
||||
|
||||
int ID = sms_sts.Ping(ID_Feetech);
|
||||
if(ID!=-1){
|
||||
Serial.print("Servo ID:");
|
||||
|
||||
145
Actionneurs/Ascenseur.ino
Normal file
145
Actionneurs/Ascenseur.ino
Normal file
@ -0,0 +1,145 @@
|
||||
#define ID_FEETECH_ASC_G 11
|
||||
#define ID_FEETECH_ASC_D 10
|
||||
|
||||
#define ASC_G_SIGNE 1
|
||||
#define ASC_D_SIGNE -1
|
||||
|
||||
enum etat_ascenseur_t{
|
||||
INIT,
|
||||
CHERCHE_BUTEE_INIT,
|
||||
CHERCHE_BUTEE,
|
||||
EA_POS_INIT,
|
||||
ACTIF
|
||||
}etat_ascenseur=CHERCHE_BUTEE_INIT;
|
||||
|
||||
void Ascenseur_init(void){
|
||||
Ascenseur_config_servo(ID_FEETECH_ASC_D);
|
||||
Ascenseur_config_servo(ID_FEETECH_ASC_G);
|
||||
}
|
||||
|
||||
void Ascenseur_config_servo(int id){
|
||||
sms_sts.unLockEprom(id); // unlock EPROM-SAFE
|
||||
sms_sts.writeByte(id, 30, 1); // Resolution à 1
|
||||
sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_L, 0); // Multitour
|
||||
sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_H, 0); // Multitour
|
||||
|
||||
sms_sts.LockEprom(id); // unlock EPROM-SAFE
|
||||
Serial.printf("Servo ID : %d\n",id);
|
||||
for(int i=0; i < 40; i++){
|
||||
Serial.printf("Memoire %d : %d\n", i, sms_sts.readByte(ID_Feetech, i));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ascenseur_gestion(void){
|
||||
u8 asc_ID[2] = {ID_FEETECH_ASC_D, ID_FEETECH_ASC_G};
|
||||
static int step=0;
|
||||
s16 tab_vitesses[2], tab_position[2];
|
||||
u16 tab_vitesses_u[2];
|
||||
u8 tab_acc[2];
|
||||
tab_acc[0] = 50;
|
||||
tab_acc[1] = 50;
|
||||
bool erreur;
|
||||
static int position_basse_gauche, position_haute_gauche;
|
||||
static int position_basse_droit, position_haute_droit;
|
||||
static bool butee_g = 0, butee_d = 0;
|
||||
|
||||
switch(etat_ascenseur){
|
||||
case INIT:
|
||||
// On vérifie que les deux servomoteurs répondent
|
||||
do{
|
||||
int ID = sms_sts.Ping(asc_ID[0]);
|
||||
erreur = sms_sts.getLastError();
|
||||
ID = sms_sts.Ping(asc_ID[1]);
|
||||
erreur += sms_sts.getLastError();
|
||||
}while(erreur != 0);
|
||||
break;
|
||||
|
||||
case CHERCHE_BUTEE_INIT:
|
||||
tab_vitesses[0] = 500 * ASC_D_SIGNE;
|
||||
tab_vitesses[1] = 500 * ASC_G_SIGNE;
|
||||
sms_sts.WheelMode(ID_FEETECH_ASC_D);
|
||||
sms_sts.WheelMode(ID_FEETECH_ASC_G);
|
||||
sms_sts.SyncWriteSpe(asc_ID, 2, tab_vitesses, tab_acc);
|
||||
delay(200);
|
||||
etat_ascenseur = CHERCHE_BUTEE;
|
||||
break;
|
||||
|
||||
case CHERCHE_BUTEE:
|
||||
{
|
||||
int my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
|
||||
if(abs(my_load) > 150 ){
|
||||
sms_sts.EnableTorque(ID_FEETECH_ASC_D, 0);
|
||||
position_basse_droit = sms_sts.ReadPos(ID_FEETECH_ASC_D);
|
||||
Serial.printf("position_basse_droit:%d\n", position_basse_droit);
|
||||
butee_d = true;
|
||||
}
|
||||
|
||||
my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
|
||||
if(abs(my_load) > 150 ){
|
||||
sms_sts.EnableTorque(ID_FEETECH_ASC_G, 0);
|
||||
position_basse_gauche = sms_sts.ReadPos(ID_FEETECH_ASC_G);
|
||||
Serial.printf("position_basse_gauche:%d\n", position_basse_gauche);
|
||||
butee_g = true;
|
||||
}
|
||||
|
||||
if(butee_d && butee_g){
|
||||
etat_ascenseur = EA_POS_INIT;
|
||||
delay(500);
|
||||
}
|
||||
break;}
|
||||
|
||||
case EA_POS_INIT:
|
||||
sms_sts.ServoMode(ID_FEETECH_ASC_D);
|
||||
sms_sts.ServoMode(ID_FEETECH_ASC_G);
|
||||
tab_position[0] = position_basse_droit + (-250 * ASC_D_SIGNE); // Droit
|
||||
tab_position[1] = position_basse_gauche + (-250 * ASC_G_SIGNE); // Gauche
|
||||
position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE);
|
||||
position_haute_gauche = position_basse_gauche - (8100 * ASC_G_SIGNE);
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
etat_ascenseur = ACTIF;
|
||||
break;
|
||||
|
||||
case ACTIF:
|
||||
etat_ascenseur = ACTIF;
|
||||
if (Serial.available() > 0) {
|
||||
// get incoming byte:
|
||||
int inByte = 0;
|
||||
inByte = Serial.read();
|
||||
if(inByte == 'd'){
|
||||
step++;
|
||||
}
|
||||
if(inByte == 'u'){
|
||||
step--;
|
||||
}
|
||||
while(Serial.available() > 0){
|
||||
inByte = Serial.read();
|
||||
}
|
||||
tab_position[0] = position_basse_droit + (250 + step * 500)* (ASC_D_SIGNE); // Droit
|
||||
tab_position[1] = position_basse_gauche + (250 + step * 500)* (ASC_G_SIGNE); // Gauche
|
||||
Serial.printf("position_droit:%d\n", tab_position[0]);
|
||||
Serial.printf("position_gauche:%d\n", tab_position[1]);
|
||||
if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) > 1){
|
||||
tab_position[0] = position_haute_droit;
|
||||
}
|
||||
if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) < 0){
|
||||
tab_position[0] = position_basse_droit;
|
||||
}
|
||||
if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) > 1){
|
||||
tab_position[1] = position_haute_gauche;
|
||||
}
|
||||
if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) < 0){
|
||||
tab_position[1] = position_basse_gauche;
|
||||
}
|
||||
|
||||
|
||||
tab_vitesses_u[0] = 2000;
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user