/********* 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 	//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 */