575 lines
14 KiB
C++
575 lines
14 KiB
C++
#include <SCServo.h>
|
|
#include "Ascenseur.h"
|
|
|
|
#define SERVO_FOUCHE 5
|
|
#define SERVO_PINCE_GAUCHE 8
|
|
#define SERVO_PINCE_DROITE 9
|
|
|
|
#define FOURCHE_TRANSPORT 5, 277
|
|
#define FOURCHE_LEVEE 5, 206
|
|
#define FOURCHE_PRISE 5, 308
|
|
|
|
#define DOIGT_PINCE_GAUCHE_FERME 3, 1530
|
|
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
|
|
#define DOIGT_PINCE_GAUCHE_SEUIL 1545
|
|
|
|
#define DOIGT_PINCE_DROIT_FERME 4, 2500
|
|
#define DOIGT_PINCE_DROIT_OUVRE 4, 2365
|
|
#define DOIGT_PINCE_DROIT_SEUIL 1575
|
|
|
|
#define AIMANT_PINCE_DROIT_TIENT 8, 176
|
|
#define AIMANT_PINCE_DROIT_LACHE 8, 366
|
|
|
|
#define AIMANT_PINCE_GAUCHE_TIENT 9, 366
|
|
#define AIMANT_PINCE_GAUCHE_LACHE 9, 236
|
|
|
|
#define I2C_CDE_DEMANDE 0x00
|
|
#define I2C_CDE_REALISE 0x01
|
|
|
|
volatile byte * I2C_memory;
|
|
|
|
SMS_STS sms_sts;
|
|
|
|
int ID_Feetech = 4;
|
|
|
|
enum etat_action_t Actionneur_deplacement(void);
|
|
enum etat_action_t Actionneur_prise_initiale(void);
|
|
enum etat_action_t Actionneur_empile(void);
|
|
enum etat_action_t Actionneur_deplacement(void);
|
|
|
|
void setup()
|
|
{
|
|
int liste_servo_id[4]= {3, 4, 10, 11};
|
|
Serial.begin(115200);//sms舵机波特率115200
|
|
Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
|
|
sms_sts.pSerial = &Serial1;
|
|
//delay(5000);
|
|
|
|
|
|
//Servo fourche (12V)
|
|
ledcAttach(5, 50, 12);
|
|
// ledcWrite(5, 307); // Neutre
|
|
ledcWrite(5, 307); // Position de transport
|
|
//ledcWrite(5, 227); // Position levée (butée haute)
|
|
//ledcWrite(5, 345); // Position de prise
|
|
|
|
I2C_Slave_init(0x20);
|
|
I2C_memory = get_i2c_data();
|
|
|
|
/*
|
|
while(1){
|
|
ledcWrite(FOURCHE_PRISE);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_TRANSPORT);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_LEVEE);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_TRANSPORT);
|
|
delay(5000);
|
|
|
|
}*/
|
|
int error = 0;
|
|
do{
|
|
error = 0;
|
|
for (int i =0; i<4; i++){
|
|
int ID = sms_sts.Ping(liste_servo_id[i]);
|
|
printf("Servo %d: %d\n", liste_servo_id[i], ID );
|
|
if( ID != liste_servo_id[i]){
|
|
error = 1;
|
|
delay(100);
|
|
}
|
|
}
|
|
}while(error);
|
|
|
|
// Servo pinces à aimant
|
|
ledcAttach(8, 50, 12);
|
|
ledcWrite(8, 307);
|
|
|
|
ledcAttach(9, 50, 12);
|
|
ledcWrite(9, 307);
|
|
|
|
|
|
Translateur_init();
|
|
|
|
|
|
|
|
/*while(1){
|
|
|
|
ledcWrite(FOURCHE_PRISE);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_TRANSPORT);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_LEVEE);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_TRANSPORT);
|
|
delay(1000);
|
|
|
|
sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_PARKING, 2400, 50);
|
|
delay(1000);
|
|
sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_OUVRE, 2400, 50);
|
|
delay(3000);
|
|
sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_FERME, 2400, 50);
|
|
delay(5000);
|
|
sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_OUVRE, 2400, 50);
|
|
delay(5000);
|
|
|
|
}*/
|
|
Ascenseur_init();
|
|
|
|
}
|
|
|
|
void reglage_servo(int pin_servo){
|
|
static int valeur=307; // Neutre
|
|
printf("reglage_servo %d\n", pin_servo);
|
|
while(1){
|
|
if (Serial.available() > 0) {
|
|
// get incoming byte:
|
|
int inByte = 0;
|
|
inByte = Serial.read();
|
|
if(inByte == 'e'){
|
|
valeur++;
|
|
}
|
|
if(inByte == 'z'){
|
|
valeur+=10;
|
|
}
|
|
if(inByte == 'a'){
|
|
valeur+=100;
|
|
}
|
|
if(inByte == 'd'){
|
|
valeur--;
|
|
}
|
|
if(inByte == 's'){
|
|
valeur-=10;
|
|
}
|
|
if(inByte == 'f'){
|
|
valeur-=100;
|
|
}
|
|
if(inByte == 'q'){
|
|
break;
|
|
}
|
|
printf("Valeur %d\n", valeur);
|
|
ledcWrite(pin_servo, valeur);
|
|
}
|
|
Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
|
|
Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
|
|
delay(50);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void loop()
|
|
{
|
|
static int m_pos=0;
|
|
static unsigned long myTime=0;
|
|
|
|
if(millis() > myTime + 30){
|
|
myTime = millis();
|
|
Serial.print(">millis:");
|
|
Serial.println(millis());
|
|
Serial.printf(">I2C_c:%d\n>I2C_t:%d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]);
|
|
}
|
|
|
|
|
|
int Pos;
|
|
int Speed;
|
|
int Load;
|
|
int Voltage;
|
|
int Temper;
|
|
int Move;
|
|
int Current;
|
|
|
|
if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){
|
|
|
|
switch (I2C_memory[I2C_CDE_DEMANDE]){
|
|
case 0:
|
|
// Aucune commande
|
|
I2C_memory[I2C_CDE_REALISE] = 0;
|
|
break;
|
|
|
|
case 1:
|
|
// Position de départ
|
|
break;
|
|
|
|
case 2:
|
|
// Position de déplacement
|
|
// Fourche levée pour ne pas géner les capteurs
|
|
if(Actionneur_deplacement() == ACTION_TERMINEE){
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
}
|
|
break;
|
|
|
|
case 3:
|
|
// Position de prise planche
|
|
if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
}
|
|
break;
|
|
|
|
case 4:
|
|
// Prise initiale
|
|
if(Actionneur_prise_initiale() == ACTION_TERMINEE){
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
}
|
|
break;
|
|
|
|
case 5:
|
|
// Empile
|
|
if(Actionneur_empile() == ACTION_TERMINEE){
|
|
Serial.println("I2C : ACTION_TERMINEE !!!");
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
}
|
|
break;
|
|
|
|
case 6:
|
|
while(Actionneur_depile_planche() != ACTION_TERMINEE);
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
break;
|
|
|
|
case 7:
|
|
while(Actionneur_prepare_prise_extern() != ACTION_TERMINEE);
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
break;
|
|
|
|
case 8:
|
|
while(Actionneur_fourche_transport() != ACTION_TERMINEE);
|
|
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
if (Serial.available() > 0) {
|
|
// get incoming byte:
|
|
int inByte = 0;
|
|
inByte = Serial.read();
|
|
if(inByte == '?'){
|
|
// Affiche le menu
|
|
Serial.println("d: Ascenseur step down");
|
|
Serial.println("u: Ascenseur step up");
|
|
Serial.println("m: Ascenseur monte");
|
|
Serial.println("l: Ascenseur descend");
|
|
Serial.println("a: Translateur avance");
|
|
Serial.println("z: Translateur recule");
|
|
Serial.println("w: Doigt pince ouvre");
|
|
Serial.println("x: Doigt pince ferme");
|
|
Serial.println("1: Reglage servo 8");
|
|
Serial.println("2: Reglage servo 9");
|
|
Serial.println("3: Reglage servo fourche (5)");
|
|
Serial.println("v: Cycle ascenseur");
|
|
Serial.println("c: Cycle translateur");
|
|
Serial.println("h: Actionneur, position de déplacement");
|
|
Serial.println("n: Actionneur, prepare prise");
|
|
Serial.println("j: Actionneur, prise initiale");
|
|
Serial.println("k: Actionneur, empile");
|
|
|
|
|
|
|
|
}
|
|
if(inByte == 'd'){
|
|
Ascenseur_step_down();
|
|
}
|
|
if(inByte == 'u'){
|
|
Ascenseur_step_up();
|
|
}
|
|
if(inByte == 'm'){
|
|
Ascenseur_monte();
|
|
}
|
|
if(inByte == 'l'){
|
|
Ascenseur_descend();
|
|
}
|
|
if(inByte == 'a'){
|
|
Translateur_avance();
|
|
}
|
|
if(inByte == 'z'){
|
|
Translateur_recule();
|
|
}
|
|
if(inByte == 'w'){
|
|
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
|
|
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
|
|
}
|
|
if(inByte == 'x'){
|
|
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
|
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
|
}
|
|
if(inByte == 'f'){
|
|
Servo_set(FOURCHE_LEVEE);
|
|
}
|
|
if(inByte == 'y'){
|
|
while(Actionneur_depile_planche() == ACTION_EN_COURS);
|
|
}
|
|
if(inByte == '1'){
|
|
reglage_servo(8);
|
|
}
|
|
if(inByte == '2'){
|
|
reglage_servo(9);
|
|
}
|
|
if(inByte == '3'){
|
|
reglage_servo(5);
|
|
}
|
|
if(inByte == 'h'){
|
|
while(Actionneur_deplacement() == ACTION_EN_COURS);
|
|
}
|
|
if(inByte == 'j'){
|
|
while(Actionneur_prise_initiale() == ACTION_EN_COURS);
|
|
}
|
|
if(inByte == 'k'){
|
|
while(Actionneur_empile() == ACTION_EN_COURS);
|
|
}
|
|
if(inByte == 'n'){
|
|
while(Actionneur_prepare_prise_planche() == ACTION_EN_COURS);
|
|
}
|
|
/*
|
|
if(inByte == 'u'){
|
|
while(Actionneur_deplacement() == ACTION_EN_COURS);
|
|
while(Actionneur_prise_initiale() == ACTION_EN_COURS);
|
|
while(Actionneur_empile() == ACTION_EN_COURS);
|
|
}*/
|
|
if(inByte == 'v'){
|
|
Ascenseur_cycle();
|
|
}
|
|
if(inByte == 'c'){
|
|
Serial.println("Cycle translateur");
|
|
while(Serial.available() > 0){
|
|
inByte = Serial.read();
|
|
}
|
|
while(1){
|
|
Translateur_cycle();
|
|
if(Serial.available() > 0){
|
|
break;
|
|
}
|
|
Serial.printf(">GPIO2:%d\n",digitalRead(2));
|
|
Serial.printf(">GPIO10:%d\n",digitalRead(10));
|
|
delay(25);
|
|
}
|
|
}
|
|
|
|
|
|
while(Serial.available() > 0){
|
|
inByte = Serial.read();
|
|
}
|
|
}
|
|
Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
|
|
Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
|
|
Serial.printf(">GPIO2:%d\n",digitalRead(2));
|
|
Serial.printf(">GPIO10:%d\n",digitalRead(10));
|
|
Serial.printf(">OUT3:%d\n",digitalRead(3));
|
|
Serial.printf(">OUT4:%d\n",digitalRead(4));
|
|
|
|
Ascenseur_gestion();
|
|
|
|
delay(25);
|
|
}
|
|
|
|
void Servo_set(int servo, int position){
|
|
switch(servo){
|
|
case 5:
|
|
case 8:
|
|
case 9:
|
|
ledcWrite(servo, position);
|
|
break;
|
|
|
|
case 3:
|
|
case 4:
|
|
sms_sts.WritePosEx(servo, position, 2000);
|
|
break;
|
|
|
|
}
|
|
}
|
|
|
|
enum etat_action_t Actionneur_deplacement(void){
|
|
Servo_set(FOURCHE_LEVEE);
|
|
// Ascenseur en bas
|
|
Ascenseur_descend();
|
|
// Pinces internes fermées
|
|
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
|
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
|
// Aimant rentrés
|
|
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
|
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
|
// Translateur à l'arrière
|
|
Translateur_recule();
|
|
|
|
return ACTION_TERMINEE;
|
|
|
|
}
|
|
|
|
enum etat_action_t Actionneur_prepare_prise_extern(void){
|
|
Servo_set(FOURCHE_TRANSPORT);
|
|
// Ascenseur en bas
|
|
Ascenseur_descend();
|
|
// Pinces internes fermées
|
|
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
|
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
|
// Aimant sortis
|
|
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
|
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
|
// Translateur à l'arrière
|
|
Translateur_recule();
|
|
|
|
return ACTION_TERMINEE;
|
|
|
|
}
|
|
|
|
enum etat_action_t Actionneur_prepare_prise_planche(void){
|
|
Servo_set(FOURCHE_PRISE);
|
|
// Ascenseur en bas
|
|
Ascenseur_descend();
|
|
// Pinces internes fermées
|
|
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
|
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
|
// Aimant rentrés
|
|
Servo_set(AIMANT_PINCE_DROIT_TIENT);
|
|
Servo_set(AIMANT_PINCE_GAUCHE_TIENT);
|
|
// Translateur à l'arrière
|
|
Translateur_recule();
|
|
|
|
return ACTION_TERMINEE;
|
|
|
|
}
|
|
|
|
enum etat_action_t Actionneur_depile_planche(void){
|
|
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
|
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
|
delay(1000);
|
|
ledcWrite(FOURCHE_LEVEE);
|
|
delay(1000);
|
|
for(int i=0;i<20;i++){
|
|
ledcWrite(FOURCHE_PRISE);
|
|
delay(50);
|
|
ledcWrite(FOURCHE_LEVEE);
|
|
delay(200);
|
|
}
|
|
|
|
return ACTION_TERMINEE;
|
|
|
|
}
|
|
|
|
enum etat_action_t Actionneur_prise_initiale(void){
|
|
static enum etat_actionneur_t{
|
|
ACTIONNEUR_PRISE_INTERNE_1,
|
|
ACTIONNEUR_PRISE_INTERNE_2,
|
|
ACTIONNEUR_RECULE_PRISE_INTERNE_1,
|
|
ACTIONNEUR_RECULE_PRISE_INTERNE_2
|
|
} etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
|
|
|
|
switch(etat_actionneur){
|
|
|
|
case ACTIONNEUR_PRISE_INTERNE_1:
|
|
Serial.println("ACTIONNEUR_PRISE_INTERNE_1");
|
|
Translateur_avance();
|
|
|
|
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
|
|
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
|
|
etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2;
|
|
break;
|
|
|
|
case ACTIONNEUR_PRISE_INTERNE_2:
|
|
Serial.println("ACTIONNEUR_PRISE_INTERNE_2");
|
|
if(Translateur_etat() == ACTION_TERMINEE){
|
|
delay(500);
|
|
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
|
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
|
delay(500);
|
|
etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_1;
|
|
}
|
|
break;
|
|
|
|
case ACTIONNEUR_RECULE_PRISE_INTERNE_1:
|
|
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_1");
|
|
Translateur_recule();
|
|
etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_2;
|
|
break;
|
|
|
|
case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
|
|
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
|
|
if(Translateur_etat() == ACTION_TERMINEE){
|
|
//Servo_set(FOURCHE_TRANSPORT);
|
|
etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
}
|
|
Ascenseur_gestion();
|
|
return ACTION_EN_COURS;
|
|
|
|
}
|
|
|
|
enum etat_action_t Actionneur_fourche_transport(){
|
|
Servo_set(FOURCHE_TRANSPORT);
|
|
return ACTION_TERMINEE;
|
|
}
|
|
|
|
enum etat_action_t Actionneur_empile(){
|
|
static enum etat_actionneur_t{
|
|
ACTIONNEUR_LEVE,
|
|
ACTIONNEUR_LEVE_1,
|
|
ACTIONNEUR_AVANCE_PRISE_INTERNE,
|
|
ACTIONNEUR_DEPOSE_PRISE_INTERNE,
|
|
ACTIONNEUR_RANGE_TRANSLATEUR,
|
|
ACTIONNEUR_DESCEND
|
|
} etat_actionneur=ACTIONNEUR_LEVE;
|
|
|
|
switch(etat_actionneur){
|
|
case ACTIONNEUR_LEVE:
|
|
Serial.println("ACTIONNEUR_LEVE");
|
|
etat_actionneur = ACTIONNEUR_LEVE_1;
|
|
Servo_set(FOURCHE_PRISE);
|
|
Ascenseur_monte();
|
|
break;
|
|
|
|
case ACTIONNEUR_LEVE_1:
|
|
Serial.println("ACTIONNEUR_LEVE_1");
|
|
if(Ascenseur_get_etat() == ACTION_TERMINEE){
|
|
etat_actionneur = ACTIONNEUR_AVANCE_PRISE_INTERNE;
|
|
Translateur_avance();
|
|
}
|
|
break;
|
|
|
|
case ACTIONNEUR_AVANCE_PRISE_INTERNE:
|
|
Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE");
|
|
if(Translateur_etat() == ACTION_TERMINEE){
|
|
delay(500);
|
|
Ascenseur_depose();
|
|
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
|
|
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
|
|
etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
|
|
}
|
|
break;
|
|
|
|
case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
|
|
Serial.println("ACTIONNEUR_DEPOSE_PRISE_INTERNE");
|
|
if(Ascenseur_get_etat() == ACTION_TERMINEE){
|
|
delay(500);
|
|
Translateur_recule();
|
|
etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
|
|
}
|
|
break;
|
|
|
|
case ACTIONNEUR_RANGE_TRANSLATEUR:
|
|
Serial.println("ACTIONNEUR_RANGE_TRANSLATEUR");
|
|
if(Translateur_etat() == ACTION_TERMINEE){
|
|
etat_actionneur = ACTIONNEUR_DESCEND;
|
|
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
|
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
|
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
|
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
|
Ascenseur_descend();
|
|
}
|
|
break;
|
|
|
|
case ACTIONNEUR_DESCEND:
|
|
Serial.println("ACTIONNEUR_DESCEND");
|
|
if(Ascenseur_get_etat() == ACTION_TERMINEE){
|
|
etat_actionneur = ACTIONNEUR_LEVE;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
}
|
|
Ascenseur_gestion();
|
|
return ACTION_EN_COURS;
|
|
}
|