385 lines
7.7 KiB
C++
385 lines
7.7 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, 287
|
|
#define FOURCHE_LEVEE 5, 217
|
|
#define FOURCHE_PRISE 5, 327
|
|
|
|
#define DOIGT_PINCE_GAUCHE_FERME 3, 1000
|
|
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 800
|
|
#define DOIGT_PINCE_GAUCHE_SEUIL 815
|
|
|
|
/// 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 etat_action_t{
|
|
ACTION_EN_COURS,
|
|
ACTION_TERMINEE,
|
|
ACTION_ECHEC
|
|
};
|
|
|
|
enum translateur_action_t{
|
|
TRANSLATEUR_INIT,
|
|
TRANSLATEUR_AVANCE,
|
|
TRANSLATEUR_RECULE,
|
|
}translateur_action = TRANSLATEUR_INIT;
|
|
|
|
|
|
enum etat_actionneur_t{
|
|
ACTIONNEUR_DEPLACEMENT,
|
|
ACTIONNEUR_PRISE_EXTERNE,
|
|
ACTIONNEUR_DEPILE,
|
|
ACTIONNEUR_PRISE_INTERNE,
|
|
ACTIONNEUR_RECULE_PRISE_INTERNE,
|
|
ACTIONNEUR_LEVE,
|
|
ACTIONNEUR_AVANCE_PRISE_INTERNE,
|
|
ACTIONNEUR_DEPOSE_PRISE_INTERNE,
|
|
ACTIONNEUR_RANGE_TRANSLATEUR,
|
|
ACTIONNEUR_DESCEND
|
|
}
|
|
|
|
|
|
/// @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 == 'z'){
|
|
Translateur_recule();
|
|
}
|
|
if(inByte == '1'){
|
|
reglage_servo(8);
|
|
}
|
|
if(inByte == '2'){
|
|
reglage_servo(9);
|
|
}
|
|
if(inByte == '3'){
|
|
reglage_servo(5);
|
|
}
|
|
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();
|
|
}
|
|
}
|
|
|
|
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);
|
|
}*/
|
|
}
|
|
|
|
|
|
|