#include <Servo.h>
int minPulse___Base = 0; // minimum servo position
int maxPulse___Base = 180; // maximum servo position
int turnRate___Base = 3; // servo turn rate increment (larger value, faster rate)
int minPulse_Ombro = 25; // minimum servo position
int maxPulse_Ombro = 145; // maximum servo position
int turnRate_Ombro = 3; // servo turn rate increment (larger value, faster rate)
int minPulse_cotovelo = 25; // minimum servo position
int maxPulse_cotovelo = 180; // maximum servo position
int turnRate_cotovelo = 3; // servo turn rate increment (larger value, faster rate)
int minPulse_Pulso = 0; // minimum servo position
int maxPulse_Pulso = 180; // maximum servo position
int turnRate_Pulso = 3; // servo turn rate increment (larger value, faster rate)
int minPulse_Giro = 0; // minimum servo position
int maxPulse_Giro = 180; // maximum servo position
int turnRate_Giro = 3; // servo turn rate increment (larger value, faster rate)
int minPulse_Garra_1 = 30; // minimum servo position
int maxPulse_Garra_1 = 95; // maximum servo position
int turnRate_Garra_1 = 3; // servo turn rate increment (larger value, faster rate)
int minPulse_Garra_2 = 30; // minimum servo position
int maxPulse_Garra_2 = 125; // maximum servo position
int turnRate_Garra_2 = 3; // servo turn rate increment (larger value, faster rate)
int buttonPin = 13; // pin that the trigger will be connected to
/** The Arduino will calculate these values for you **/
int centerServo_Ombro;
int centerServo_cotovelo;
int centerServo_Pulso;
int centerServo_Giro;
int centerServo_Garra_1;
int centerServo_Garra_2;
int centerServo___Base;
Servo servo_Ombro;
Servo servo_cotovelo;
Servo servo_Pulso;
Servo servo_Giro;
Servo servo_Garra_1;
Servo servo_Garra_2;
Servo servo___Base;
int pulseWidth_Ombro;
int pulseWidth_cotovelo; // servo pulse width
int pulseWidth_Pulso; // servo pulse width
int pulseWidth_Giro; // servo pulse width
int pulseWidth_Garra_1; // servo pulse width
int pulseWidth_Garra_2; // servo pulse width
int pulseWidth___Base; // servo pulse width
void setup() {
pinMode(buttonPin, OUTPUT);
servo_Ombro.attach(3); //Ombro
servo_cotovelo.attach(4); //Cotovelo
servo_Pulso.attach(6); //pulso - vertical
servo_Giro.attach(5); //pulso - giro
servo_Garra_1.attach(7);
servo_Garra_2.attach(8);
servo___Base.attach(2); //- ___Base
center_arm();
Serial.begin(9600); // opens serial port, sets data rate to 9600 bps
Serial.println(" Arduino Serial Servo Control");
Serial.println("Use QWERTY/ASDFGH para controlar o Braço e Espaço pra resetar");
Serial.println();
}
void loop() {
// check for serial input
if (Serial.available() > 0) {
int data = Serial.read(); // read the incoming byte:
digitalWrite(buttonPin, LOW); // turn the pin off on any incoming data
switch(data)
{
//----
case 'q' :
pulseWidth___Base = pulseWidth___Base - turnRate___Base;
break;
case 'a' :
pulseWidth___Base = pulseWidth___Base + turnRate___Base;
break ;
//----
case 's' : {
pulseWidth_Ombro = pulseWidth_Ombro - turnRate_Ombro;
break;}
case 'w' :{
pulseWidth_Ombro = pulseWidth_Ombro + turnRate_Ombro;
break ;}
//--
case 'd' : {
pulseWidth_cotovelo = pulseWidth_cotovelo - turnRate_cotovelo;
break;}
case 'e' : {
pulseWidth_cotovelo = pulseWidth_cotovelo + turnRate_cotovelo;
break ;}
//--
case 'r' : {
pulseWidth_Pulso = pulseWidth_Pulso - turnRate_Pulso;
break;}
case 'f' :{
pulseWidth_Pulso = pulseWidth_Pulso + turnRate_Pulso;
break ;}
//--
case 't' : {
pulseWidth_Giro = pulseWidth_Giro - turnRate_Giro;
break;}
case 'g' :{
pulseWidth_Giro = pulseWidth_Giro + turnRate_Giro;
break ;}
//--
case 'y' :{
open_gripper();
break;}
case 'h' :{
close_gripper();
break ;}
//--
case ' ' : {
center_arm();
delay(30);
break;}
case '.' : {
delay(1000);
break;}
//--
}
// stop servo pulse at min and max
if (pulseWidth_Ombro > maxPulse_Ombro) {
pulseWidth_Ombro = maxPulse_Ombro;
}
if (pulseWidth_Ombro < minPulse_Ombro) {
pulseWidth_Ombro = minPulse_Ombro;
}
// stop servo pulse at min and max
if (pulseWidth_cotovelo > maxPulse_cotovelo) {
pulseWidth_cotovelo = maxPulse_cotovelo;
}
if (pulseWidth_cotovelo < minPulse_cotovelo) {
pulseWidth_cotovelo = minPulse_cotovelo;
}
// stop servo pulse at min and max
if (pulseWidth_Pulso > maxPulse_Pulso) {
pulseWidth_Pulso = maxPulse_Pulso;
}
if (pulseWidth_Pulso < minPulse_Pulso) {
pulseWidth_Pulso = minPulse_Pulso;
}
// stop servo pulse at min and max
if (pulseWidth_Giro > maxPulse_Giro) {
pulseWidth_Giro = maxPulse_Giro;
}
if (pulseWidth_Giro < minPulse_Giro) {
pulseWidth_Giro = minPulse_Giro;
}
// stop servo pulse at min and max
if (pulseWidth_Garra_1 > maxPulse_Garra_1) {
pulseWidth_Garra_1 = maxPulse_Garra_1;
}
if (pulseWidth_Garra_1 < minPulse_Garra_1) {
pulseWidth_Garra_1 = minPulse_Garra_1;
}
if (pulseWidth_Garra_2 > maxPulse_Garra_2) {
pulseWidth_Garra_2 = maxPulse_Garra_2;
}
if (pulseWidth_Garra_2 < minPulse_Garra_2) {
pulseWidth_Garra_2 = minPulse_Garra_2;
}
if (pulseWidth___Base > maxPulse___Base) {
pulseWidth___Base = maxPulse___Base;
}
if (pulseWidth___Base < minPulse___Base) {
pulseWidth___Base = minPulse___Base;
}
servo___Base.write(pulseWidth___Base);
servo_Ombro.write(pulseWidth_Ombro);
servo_cotovelo.write(pulseWidth_cotovelo);
servo_Pulso.write(pulseWidth_Pulso);
servo_Giro.write(pulseWidth_Giro);
servo_Garra_1.write(pulseWidth_Garra_1);
servo_Garra_2.write(pulseWidth_Garra_2);
// print pulseWidth back to the Serial Monitor (uncomment to debug)
Serial.print(" Servo _Ombro: ");
Serial.print(pulseWidth_Ombro);
Serial.print(" Servo _cotovelo: ");
Serial.print(pulseWidth_cotovelo);
Serial.print(" Servo _Pulso: ");
Serial.print(pulseWidth_Pulso);
Serial.print(" Servo _Giro: ");
Serial.print(pulseWidth_Giro);
Serial.print(" Servo _Garra_1: ");
Serial.print(pulseWidth_Garra_1);
Serial.print(" Servo _Garra_2: ");
Serial.print(pulseWidth_Garra_2);
Serial.print(" Servo ___Base: ");
Serial.print(pulseWidth___Base);
Serial.println(" degrees");
}
}
//-------------
void center_arm(){
servo_Pulso.write(65);
pulseWidth_Pulso = 65;
delay(15);
servo_Ombro.write(145);
pulseWidth_Ombro = 145;
delay(15);
servo_cotovelo.write(30);
pulseWidth_cotovelo = 30;
delay(15);
servo_Giro.write(165);
pulseWidth_Giro = 165;
delay(15);
servo_Garra_1.write(95);
pulseWidth_Garra_1 = 95;
delay(15);
servo_Garra_2.write(45);
pulseWidth_Garra_2 = 45;
delay(15);
servo___Base.write(90);
pulseWidth___Base = 90;
delay(15);
};
void open_gripper(){
pulseWidth_Garra_1 = 125;
pulseWidth_Garra_2 = 45;
}
void close_gripper(){
pulseWidth_Garra_1 = 65;
pulseWidth_Garra_2 = 95;
// servo_Garra_1.write(65);
}
Nenhum comentário:
Postar um comentário