"Servo Control" rev_01

/********* 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 09:29:57

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* Fix the code */
/****** 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);

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t Base_Servomotor_PWMSignal_PIN_D3      = 3;
const uint8_t Braccio1_Servomotor_PWMSignal_PIN_D5      = 5;
const uint8_t Pinza_Servomotor_PWMSignal_PIN_D6     = 6;
const uint8_t Polso_Servomotor_PWMSignal_PIN_D9     = 9;
const uint8_t Rotazione_Servomotor_PWMSignal_PIN_D10        = 10;

/***** DEFINITION OF OUTPUT RAW VARIABLES *****/
uint8_t Base_Servomotor_PWMSignal_PIN_D3_rawData        = 0;
uint8_t Braccio1_Servomotor_PWMSignal_PIN_D5_rawData        = 0;
uint8_t Pinza_Servomotor_PWMSignal_PIN_D6_rawData       = 0;
uint8_t Polso_Servomotor_PWMSignal_PIN_D9_rawData       = 0;
uint8_t Rotazione_Servomotor_PWMSignal_PIN_D10_rawData      = 0;

/***** DEFINITION OF OUTPUT PHYSICAL VARIABLES *****/
float   Base_Servomotor_PWMSignal_PIN_D3_phyData        = 0.0;
float   Braccio1_Servomotor_PWMSignal_PIN_D5_phyData        = 0.0;
float   Pinza_Servomotor_PWMSignal_PIN_D6_phyData       = 0.0;
float   Polso_Servomotor_PWMSignal_PIN_D9_phyData       = 0.0;
float   Rotazione_Servomotor_PWMSignal_PIN_D10_phyData      = 0.0;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/

// Creating servo objects
Servo servoBase;
Servo servoBraccio1;
Servo servoPolso;
Servo servoPinza;

// Defining PWM pins for the servos
#define PIN_SERVO_BASE 3
#define PIN_SERVO_BRACCIO1 5
#define PIN_SERVO_POLSO 6
#define PIN_SERVO_PINZA 10

// Initial positions of the servos (in degrees)
int posizioneBase = 90;
int posizioneBraccio1 = 120;
int posizionePolso = 115;
int posizionePinza = 145;

// Function to constrain angle values
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_Servomotor_PWMSignal_PIN_D3,    OUTPUT);
    pinMode(Braccio1_Servomotor_PWMSignal_PIN_D5,    OUTPUT);
    pinMode(Pinza_Servomotor_PWMSignal_PIN_D6,   OUTPUT);
    pinMode(Polso_Servomotor_PWMSignal_PIN_D9,   OUTPUT);
    pinMode(Rotazione_Servomotor_PWMSignal_PIN_D10,  OUTPUT);

    Serial.begin(9600);

    // Associating servos to their respective pins
    servoBase.attach(PIN_SERVO_BASE);
    servoBraccio1.attach(PIN_SERVO_BRACCIO1);
    servoPolso.attach(PIN_SERVO_POLSO);
    servoPinza.attach(PIN_SERVO_PINZA);

    // Initial positioning of the servos
    servoBase.write(posizioneBase);
    servoBraccio1.write(posizioneBraccio1);
    servoPolso.write(posizionePolso);
    servoPinza.write(posizionePinza);

    Serial.println("Sistema pronto. Usa i tasti per controllare i servo:");
    Serial.println("q/a - Base");
    Serial.println("w/s - Braccio 1");
    Serial.println("e/d - Polso");
    Serial.println("t/g - Pinza");
}

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

    // Check if a command has been received via serial
    if (Serial.available() > 0) {
        char comando = Serial.read();

        // Update the servo position based on the received command
        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 '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;
        }

        // Update the servos to the new positions
        servoBase.write(posizioneBase);
        servoBraccio1.write(posizioneBraccio1);
        servoPolso.write(posizionePolso);
        servoPinza.write(posizionePinza);

        // Print the new servo position for monitoring
        Serial.print("Base: ");
        Serial.print(posizioneBase);
        Serial.print(" | Braccio1: ");
        Serial.print(posizioneBraccio1);
        Serial.print(" | Polso: ");
        Serial.print(posizionePolso);
        Serial.print(" | Pinza: ");
        Serial.println(posizionePinza);
    }
}

void updateOutputs()
{
    analogWrite(Base_Servomotor_PWMSignal_PIN_D3, Base_Servomotor_PWMSignal_PIN_D3_rawData);
    analogWrite(Braccio1_Servomotor_PWMSignal_PIN_D5, Braccio1_Servomotor_PWMSignal_PIN_D5_rawData);
    analogWrite(Pinza_Servomotor_PWMSignal_PIN_D6, Pinza_Servomotor_PWMSignal_PIN_D6_rawData);
    analogWrite(Polso_Servomotor_PWMSignal_PIN_D9, Polso_Servomotor_PWMSignal_PIN_D9_rawData);
    analogWrite(Rotazione_Servomotor_PWMSignal_PIN_D10, Rotazione_Servomotor_PWMSignal_PIN_D10_rawData);
}

/* END CODE */