"Servo Control" rev_02

/********* Pleasedontcode.com **********

    Pleasedontcode thanks you for automatic code generation! Enjoy your code!

    - Terms and Conditions:
    You have a non-exclusive, revocable, worldwide, royalty-free license
    for personal and commercial use. Attribution is optional; modifications
    are allowed, but you're responsible for code maintenance. We're not
    liable for any loss or damage. For full terms,
    please visit pleasedontcode.com/termsandconditions.

    - Project: "Servo Control"
    - Source Code NOT compiled for: Arduino Uno
    - Source Code created on: 2024-11-29 11:10:35

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* Convertire l'angolo del potenziometro in gradi per */
    /* il servo */
/****** END SYSTEM REQUIREMENTS *****/

/* START CODE */

/****** DEFINITION OF LIBRARIES *****/
#include <Servo.h>  //https://github.com/arduino-libraries/Servo

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);
void updateOutputs(void);

/***** DEFINITION OF ANALOG INPUT PINS *****/
const uint8_t Base_Potentiometer_Vout_PIN_A0        = A0;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t Base_Servomotor_PWMSignal_PIN_D3      = 3;
const uint8_t Base_Servomotor_PWMSignal_PIN_D5      = 5; // Added for servoBraccio1
const uint8_t Base_Servomotor_PWMSignal_PIN_D6      = 6; // Added for servoPolso
const uint8_t Base_Servomotor_PWMSignal_PIN_D9      = 9; // Added for servoS
const uint8_t Base_Servomotor_PWMSignal_PIN_D10     = 10; // Added for servoPinza

/***** DEFINITION OF OUTPUT RAW VARIABLES *****/
/***** used to store raw data *****/
uint8_t Base_Servomotor_PWMSignal_PIN_D3_rawData        = 0;

/***** DEFINITION OF OUTPUT PHYSICAL VARIABLES *****/
/***** used to store data after characteristic curve transformation *****/
float   Base_Servomotor_PWMSignal_PIN_D3_phyData        = 0.0;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Servo servoBase; // Added for servo control
Servo servoBraccio1; // Added for servo control
Servo servoPolso; // Added for servo control
Servo servoS; // Added for servo control
Servo servoPinza; // Added for servo control

// Posizioni iniziali dei servo (in gradi)
int posizioneBase = 90; // Added for servo control
int posizioneBraccio1 = 120; // Added for servo control
int posizionePolso = 115; // Added for servo control
int posizioneS = 90; // Added for servo control
int posizionePinza = 145; // Added for servo control

// Funzione per limitare i valori degli angoli
int constrainAngle(int angolo, int minimo, int massimo) {
  return constrain(angolo, minimo, massimo);
}

void setup(void)
{
    // put your setup code here, to run once:

    pinMode(Base_Potentiometer_Vout_PIN_A0, INPUT);
    pinMode(Base_Servomotor_PWMSignal_PIN_D3,    OUTPUT);

    // Associazione dei servomotori ai rispettivi pin
    servoBase.attach(Base_Servomotor_PWMSignal_PIN_D3);
    servoBraccio1.attach(Base_Servomotor_PWMSignal_PIN_D5);
    servoPolso.attach(Base_Servomotor_PWMSignal_PIN_D6);
    servoS.attach(Base_Servomotor_PWMSignal_PIN_D9);
    servoPinza.attach(Base_Servomotor_PWMSignal_PIN_D10);

    // Posizionamento iniziale dei servo
    servoBase.write(posizioneBase);
    servoBraccio1.write(posizioneBraccio1);
    servoPolso.write(posizionePolso);
    servoS.write(posizioneS);
    servoPinza.write(posizionePinza);

    Serial.begin(9600); // Added for serial communication
    Serial.println("Sistema pronto. Usa i tasti per controllare i servo:"); // Added for user instructions
    Serial.println("q/a - Base"); // Added for user instructions
    Serial.println("w/s - Braccio 1"); // Added for user instructions
    Serial.println("e/d - Polso"); // Added for user instructions
    Serial.println("r/f - Rotazione Polso (S)"); // Added for user instructions
    Serial.println("t/g - Pinza"); // Added for user instructions
}

void loop(void)
{
    // put your main code here, to run repeatedly:
    updateOutputs(); // Refresh output data

    // Read the potentiometer value and convert it to an angle for the servo
    int potValue = analogRead(Base_Potentiometer_Vout_PIN_A0); // Read the potentiometer
    int angle = map(potValue, 0, 1023, 0, 180); // Map the potentiometer value to an angle (0-180 degrees)
    servoBase.write(angle); // Write the angle to the servo

    // Controlla se è stato ricevuto un comando tramite seriale
    if (Serial.available() > 0) {
        char comando = Serial.read();

        // Aggiorna la posizione del servo in base al comando ricevuto
        switch (comando) {
            case 'q':
                posizioneBase = constrainAngle(posizioneBase + 2, 0, 180);
                break;
            case 'a':
                posizioneBase = constrainAngle(posizioneBase - 2, 0, 180);
                break;
            case 'w':
                posizioneBraccio1 = constrainAngle(posizioneBraccio1 + 2, 100, 140);
                break;
            case 's':
                posizioneBraccio1 = constrainAngle(posizioneBraccio1 - 2, 100, 140);
                break;
            case 'e':
                posizionePolso = constrainAngle(posizionePolso + 2, 100, 130);
                break;
            case 'd':
                posizionePolso = constrainAngle(posizionePolso - 2, 100, 130);
                break;
            case 'r':
                posizioneS = constrainAngle(posizioneS + 2, 0, 180);
                break;
            case 'f':
                posizioneS = constrainAngle(posizioneS - 2, 0, 180);
                break;
            case 't':
                posizionePinza = constrainAngle(posizionePinza + 2, 120, 175);
                break;
            case 'g':
                posizionePinza = constrainAngle(posizionePinza - 2, 120, 175);
                break;
            default:
                Serial.println("Comando non valido. Usa i tasti indicati.");
                break;
        }

        // Aggiorna i servo alle nuove posizioni
        servoBase.write(posizioneBase);
        servoBraccio1.write(posizioneBraccio1);
        servoPolso.write(posizionePolso);
        servoS.write(posizioneS);
        servoPinza.write(posizionePinza);

        // Stampa la nuova posizione del servo per monitoraggio
        Serial.print("Base: ");
        Serial.print(posizioneBase);
        Serial.print(" | Braccio1: ");
        Serial.print(posizioneBraccio1);
        Serial.print(" | Polso: ");
        Serial.print(posizionePolso);
        Serial.print(" | Rotazione Polso (S): ");
        Serial.print(posizioneS);
        Serial.print(" | Pinza: ");
        Serial.println(posizionePinza);
    }
}

void updateOutputs()
{
    analogWrite(Base_Servomotor_PWMSignal_PIN_D3, Base_Servomotor_PWMSignal_PIN_D3_rawData);
}

/* END CODE */