96 lines
1.7 KiB
Arduino
96 lines
1.7 KiB
Arduino
|
// Controlling a servo position with speed control
|
||
|
|
// by Alex Posada
|
||
|
|
|
||
|
|
#include <Servo.h>
|
||
|
|
|
||
|
|
#define debug 1
|
||
|
|
|
||
|
|
Servo myservo[5];
|
||
|
|
|
||
|
|
unsigned long timer[5] = {
|
||
|
|
0,0,0,0,0};
|
||
|
|
unsigned long prevTimer[5] = {
|
||
|
|
0,0,0,0,0};
|
||
|
|
|
||
|
|
int servoStep[5] = {
|
||
|
|
1,1,1,1,1};
|
||
|
|
int servoSpeed[5] ={
|
||
|
|
20,20,20,20,20}; // total time = ( servoSpeed/servoStep ) * 180
|
||
|
|
|
||
|
|
int compass[5] = {
|
||
|
|
2,3,4,5,6};
|
||
|
|
int servoPin[5] = {
|
||
|
|
7,8,9,10,11};
|
||
|
|
int sensor[5] = {
|
||
|
|
0,0,0,0,0};
|
||
|
|
int servoPos[5] = {
|
||
|
|
0,0,0,0,0};
|
||
|
|
int servoMax[5] = {
|
||
|
|
179,179,179,179,179};
|
||
|
|
int servoMin[5] = {
|
||
|
|
0,0,0,0,0};
|
||
|
|
boolean servoActive[5] = {
|
||
|
|
0,0,0,0,0};
|
||
|
|
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
|
||
|
|
for(int i=0 ; i<5 ; i++)
|
||
|
|
{
|
||
|
|
myservo[i].attach(servoPin[i]); // attaches the servo on pin 9 to the servo object
|
||
|
|
pinMode(compass[i], INPUT);
|
||
|
|
digitalWrite(compass[i], HIGH);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
Serial.begin(9600);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
|
||
|
|
for(int i=0 ; i<5 ; i++)
|
||
|
|
{
|
||
|
|
sensor[i] = digitalRead(compass[i]); // convertirlo en una funcion FOR con arrays por cada servo
|
||
|
|
if(!sensor[i])
|
||
|
|
servoActive[i] = true;
|
||
|
|
|
||
|
|
if(servoActive[i])
|
||
|
|
{
|
||
|
|
|
||
|
|
timer[i] = millis();
|
||
|
|
if(timer[i] - prevTimer[i] > servoSpeed[i])
|
||
|
|
{
|
||
|
|
servoPos[i] = servoPos[i] + servoStep[i];
|
||
|
|
|
||
|
|
if(debug)
|
||
|
|
{
|
||
|
|
Serial.print("Servo");
|
||
|
|
Serial.print(i+1);
|
||
|
|
Serial.print(" = ");
|
||
|
|
Serial.println(servoPos[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
myservo[i].write(servoPos[i]);
|
||
|
|
if(servoPos[i] >= servoMax[i])
|
||
|
|
{
|
||
|
|
myservo[i].write(servoMin[i]);
|
||
|
|
servoStep[i] = -servoStep[i];
|
||
|
|
}
|
||
|
|
else if(servoPos[i] == 0)
|
||
|
|
{
|
||
|
|
servoStep[i] = -servoStep[i];
|
||
|
|
servoActive[i] = false;
|
||
|
|
}
|
||
|
|
prevTimer[i] = timer[i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|