Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
a650bacca9
Robot2025/Actionneurs/Actionneurs.ino

519 lines
12 KiB
C++
Raw Blame History

#include <SCServo.h>
#include "Ascenseur.h"
#define SERVO_FOUCHE 5
#define SERVO_PINCE_GAUCHE 8
#define SERVO_PINCE_DROITE 9
#define FOURCHE_TRANSPORT 5, 287
#define FOURCHE_LEVEE 5, 217
#define FOURCHE_PRISE 5, 327
#define DOIGT_PINCE_GAUCHE_FERME 3, 1500
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842
#define DOIGT_PINCE_GAUCHE_SEUIL 1515
/// TODO renseigner de vraies valeurs
#define DOIGT_PINCE_DROIT_FERME 4, 1000
#define DOIGT_PINCE_DROIT_OUVRE 4, 800
#define DOIGT_PINCE_DROIT_SEUIL 815
/// FIN TODO
#define AIMANT_PINCE_DROIT_TIENT 8, 147
#define AIMANT_PINCE_DROIT_LACHE 8, 337
#define AIMANT_PINCE_GAUCHE_TIENT 9, 500
#define AIMANT_PINCE_GAUCHE_LACHE 9, 400
SMS_STS sms_sts;
int ID_Feetech = 4;
struct position_t{
int nb_tour;
uint position;
} cible_haute, cible_basse, actuelle;
enum translateur_action_t{
TRANSLATEUR_INIT,
TRANSLATEUR_AVANCE,
TRANSLATEUR_RECULE,
}translateur_action = TRANSLATEUR_INIT;
enum etat_action_t Actionneur_empile();
/// @brief pilote la vitesse des moteurs
/// @param vitesse vitesse signée, sera saturée à 255
void Moteur_set(int vitesse){
ledcWrite(3, 0);
ledcWrite(4, 0);
if(vitesse < 0){
vitesse = -vitesse;
if(vitesse > 255){
vitesse = 255;
}
ledcWrite(3, vitesse);
}else{
if(vitesse > 255){
vitesse = 255;
}
ledcWrite(4, vitesse);
}
}
void Translateur_avance(){
Moteur_set(-255);
translateur_action = TRANSLATEUR_AVANCE;
}
void Translateur_recule(){
Moteur_set(255);
translateur_action = TRANSLATEUR_RECULE;
}
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);
cible_haute.nb_tour = 0;
cible_haute.position = 500;
cible_basse.nb_tour = 2;
cible_basse.position = 1500;
actuelle.nb_tour = 0;
//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
/*
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);
}*/
/*
sms_sts.WheelMode(10);
sms_sts.WriteSpe(10, 2000, 50);
while(1){
printf(">load_d:%d\n", sms_sts.ReadLoad(10));
delay(25);
}*/
if(sms_sts.FeedBack(ID_Feetech)!=-1){
actuelle.position = sms_sts.ReadPos(-1);
}else{
do{
Serial.println("Erreur lecture");
delay(1000);
}while(sms_sts.FeedBack(ID_Feetech) ==-1);
actuelle.position = sms_sts.ReadPos(-1);
}
Ascenseur_init();
}
void Translateur_init(void){
// Moteur
ledcAttach(3, 500, 8);
ledcWrite(3, 0);
ledcAttach(4, 500, 8);
ledcWrite(4, 0);
// Etat des contacteurs de fin de course
// relecture de 3 sur 2 (Avant)
// relecture de 4 sur 10 (Arrière)
pinMode(2, INPUT);
pinMode(10, INPUT);
}
enum etat_action_t Translateur_etat(void){
if(translateur_action == TRANSLATEUR_AVANCE){
if(digitalRead(2) == 0){
return ACTION_TERMINEE;
}
}
if(translateur_action == TRANSLATEUR_RECULE){
if(digitalRead(10) == 0){
return ACTION_TERMINEE;
}
}
return ACTION_EN_COURS;
}
void Translateur_cycle(void){
if(Translateur_etat() == ACTION_TERMINEE){
if(translateur_action == TRANSLATEUR_RECULE){
Translateur_avance();
}else{
Translateur_recule();
}
}
}
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;
static position_t cible;
if(millis() > myTime + 100){
myTime = millis();
Serial.print(">millis:");
Serial.println(millis());
}
int Pos;
int Speed;
int Load;
int Voltage;
int Temper;
int Move;
int Current;
if (Serial.available() > 0) {
// get incoming byte:
int inByte = 0;
inByte = Serial.read();
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 == 'w'){
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
}
if(inByte == 'x'){
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
}
if(inByte == 'z'){
Translateur_recule();
}
if(inByte == '1'){
reglage_servo(8);
}
if(inByte == '2'){
reglage_servo(9);
}
if(inByte == '3'){
reglage_servo(5);
}
if(inByte == 'u'){
while(Actionneur_empile() == ACTION_EN_COURS);
}
if(inByte == 'c'){
while(Serial.available() > 0){
inByte = Serial.read();
}
while(1){
Translateur_cycle();
if(Serial.available() > 0){
break;
}
printf(">GPIO2:%d\n",digitalRead(2));
printf(">GPIO10:%d\n",digitalRead(10));
}
}
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));
Ascenseur_gestion();
/*
if(sms_sts.FeedBack(ID)!=-1){
Pos = sms_sts.ReadPos(-1);
Speed = sms_sts.ReadSpeed(-1);
Load = sms_sts.ReadLoad(-1);
Voltage = sms_sts.ReadVoltage(-1);
Temper = sms_sts.ReadTemper(-1);
Move = sms_sts.ReadMove(-1);
Current = sms_sts.ReadCurrent(-1);
Serial.print(">Position:");
Serial.println(Pos);
Serial.print(">Speed:");
Serial.println(Speed);
Serial.print(">Load:");
Serial.println(Load);
Serial.print(">Voltage:");
Serial.println(Voltage);
Serial.print(">Temper:");
Serial.println(Temper);
Serial.print(">Move:");
Serial.println(Move);
Serial.print(">Current:");
Serial.println(Current);
delay(50);
}else{
Serial.println("FeedBack err");
delay(500);
}*/
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_empile(){
static enum etat_actionneur_t{
ACTIONNEUR_DEPLACEMENT,
ACTIONNEUR_PRISE_EXTERNE,
ACTIONNEUR_DEPILE,
ACTIONNEUR_PRISE_INTERNE_1,
ACTIONNEUR_PRISE_INTERNE_2,
ACTIONNEUR_RECULE_PRISE_INTERNE_1,
ACTIONNEUR_RECULE_PRISE_INTERNE_2,
ACTIONNEUR_LEVE,
ACTIONNEUR_AVANCE_PRISE_INTERNE,
ACTIONNEUR_DEPOSE_PRISE_INTERNE,
ACTIONNEUR_RANGE_TRANSLATEUR,
ACTIONNEUR_DESCEND
} etat_actionneur=ACTIONNEUR_DEPLACEMENT;
switch(etat_actionneur){
case ACTIONNEUR_DEPLACEMENT:
Servo_set(FOURCHE_TRANSPORT);
Translateur_recule();
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
Servo_set(DOIGT_PINCE_DROIT_FERME);
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
delay(5000);
etat_actionneur = ACTIONNEUR_PRISE_EXTERNE;
break;
case ACTIONNEUR_PRISE_EXTERNE:
Servo_set(FOURCHE_PRISE);
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
// Attente avance
delay(5000);
etat_actionneur = ACTIONNEUR_DEPILE;
break;
case ACTIONNEUR_DEPILE:
Servo_set(FOURCHE_LEVEE);
etat_actionneur = ACTIONNEUR_PRISE_INTERNE_1;
break;
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){
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();
delay(1000);
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){
etat_actionneur = ACTIONNEUR_LEVE;
Ascenseur_monte();
}
break;
case ACTIONNEUR_LEVE:
Serial.println("ACTIONNEUR_LEVE");
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){
etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
}
break;
case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
delay(500);
Translateur_recule();
etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
break;
case ACTIONNEUR_RANGE_TRANSLATEUR:
if(Translateur_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_DESCEND;
Servo_set(DOIGT_PINCE_DROIT_FERME);
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
Ascenseur_monte();
}
break;
case ACTIONNEUR_DESCEND:
if(Ascenseur_get_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_DEPLACEMENT;
Servo_set(FOURCHE_TRANSPORT);
return ACTION_TERMINEE;
}
break;
}
Ascenseur_gestion();
return ACTION_EN_COURS;
}
Reference in New Issue View Git Blame Copy Permalink