Servo Hold Position with joystick input

[Benyhafkamp]

Servo Hold Position with joystick input - Using Arduino / Programming Questions - Arduino Forum

[Benyhafkamp]

yes thats the idea

[Benyhafkamp]

I finally got a working code

here it is, but one problem I still have is the swing of the crane , the kids figured out how to swing the load by pushing the joystick left right to start a wrecking ball effect
was wondering how to put in a 1 sec delay before the opposite direction change happens
any help would be much appreciated

#include <Servo.h>


#define SERVO_X_PIN 2                           
#define SERVO_Y_PIN 3                          


#define JOYSTICK_X_PIN A0                       
#define JOYSTICK_Y_PIN A1                      

const long MIN_PULSE = 440;                     
const long MAX_PULSE = 2500;                   
const long MIN_POT = 0;                         
const long MAX_POT = 1200;                      
const long POWER_OF_TWO_TO_AVERAGE = 4;       


const long SERVO_PULSE_RANGE = MAX_PULSE - MIN_PULSE; // 
const long START_PULSE_X = MIN_PULSE + (SERVO_PULSE_RANGE / 2);  
const long START_PULSE_Y = MIN_PULSE + (SERVO_PULSE_RANGE / 2); 

const long POT_RANGE = MAX_POT - MIN_POT;

const int BUFFER_SIZE = 1 << POWER_OF_TWO_TO_AVERAGE; 
const int BUFFER_LIMIT = BUFFER_SIZE - 1;            


const unsigned long ANALOG_READ_PERIOD = 5000;  
const unsigned long DEBUG_PERIOD = 100000;  
const unsigned long SERVO_PERIOD = 20000; 

const long LOW_CENTER_THRESHOLD = 565;   
const long HIGH_CENTER_THRESHOLD = 580;    
const long POT_TO_SPEED_CONSTANT = 55;   

long averagingBufferX[BUFFER_SIZE];
long averagingBufferY[BUFFER_SIZE];
int bufferIndex = 0;
long servoPosition[] = {START_PULSE_X, START_PULSE_Y};

long bufferTotalX = 0; 
long bufferTotalY = 0; 

unsigned long lastDebug;
unsigned long lastServo;
unsigned long lastAnalogRead;

Servo myServo[2];


void setup()
{
  Serial.begin(115200);
  myServo[0].writeMicroseconds(START_PULSE_X);
  myServo[1].writeMicroseconds(START_PULSE_Y);
  myServo[0].attach(SERVO_X_PIN, MIN_PULSE, MAX_PULSE);
  myServo[1].attach(SERVO_Y_PIN, MIN_PULSE, MAX_PULSE);

  for (byte i; i < BUFFER_SIZE; i++) 
  {
    averagingBufferX[i] = (MAX_POT - MIN_POT) / 2;
    averagingBufferY[i] = (MAX_POT - MIN_POT) / 2;
    bufferTotalX += averagingBufferX[i];
    bufferTotalY += averagingBufferY[i];
  }
  
  lastDebug = micros();
  lastServo = lastDebug;
  lastAnalogRead = lastDebug;
}

void loop()
{
  checkAnalogReadTime();
}

void checkAnalogReadTime()
{
  if (micros() - lastAnalogRead > ANALOG_READ_PERIOD)
  {
    lastAnalogRead += ANALOG_READ_PERIOD;
    long joystickInputX = analogRead(JOYSTICK_X_PIN);
    
    long joystickInputY = analogRead(JOYSTICK_Y_PIN);
    
    
    bufferIndex++;
    bufferIndex &= BUFFER_LIMIT;
   
    bufferTotalX -= averagingBufferX[bufferIndex]; 
    bufferTotalY -= averagingBufferY[bufferIndex];
    averagingBufferX[bufferIndex] = joystickInputX;
    averagingBufferY[bufferIndex] = joystickInputY;
    bufferTotalX += averagingBufferX[bufferIndex]; 
    bufferTotalY += averagingBufferY[bufferIndex];
   
    checkServoTime();
  }
}


void checkServoTime()

{
  if (micros() - lastServo > SERVO_PERIOD)
  {
    lastServo += SERVO_PERIOD;
    controlServo();
  }
}

void controlServo()

{

  long average[3];
  long servoSpeed[3];
  
  average[0] = bufferTotalX >> POWER_OF_TWO_TO_AVERAGE; // it might be a good idea to make averageX global so it can be used elsewhere in program
  average[1] = bufferTotalY >> POWER_OF_TWO_TO_AVERAGE;

  for (int i = 0; i < 2; i++)
  {
    if (average[i] < LOW_CENTER_THRESHOLD)
    {
      servoSpeed[i] = (average[i] - LOW_CENTER_THRESHOLD) / POT_TO_SPEED_CONSTANT;
      
    }
    else if  (average[i] > HIGH_CENTER_THRESHOLD)
    {
      servoSpeed[i] = (average[i] - HIGH_CENTER_THRESHOLD) / POT_TO_SPEED_CONSTANT;
                    
    }
    else 
    {
      servoSpeed[i] = 0;
    }
    servoPosition[i] += servoSpeed[i];
    if (servoPosition[i] > MAX_PULSE)
    {
      servoPosition[i] = MAX_PULSE;
    }
    else if (servoPosition[i] < MIN_PULSE)
    {
      servoPosition[i] = MIN_PULSE;
    }
    myServo[i].writeMicroseconds(servoPosition[i]);
  }

  checkDebugTime(average[0], average[1], servoPosition[0], servoPosition[1], servoSpeed[0], servoSpeed[1]);
}

void checkDebugTime(long averageX, long averageY, long servoOutputX, long servoOutputY, long speedX, long speedY)



{
  if (micros() - lastDebug > DEBUG_PERIOD)
  {
    lastDebug += DEBUG_PERIOD;

    Serial.print(F("average = "));
    Serial.print(averageX, DEC);
    Serial.print(F(", "));
    Serial.print(averageY, DEC);
    Serial.print(F(", Servo = "));
    Serial.print(servoOutputX, DEC);
    Serial.print(F(", "));
    Serial.print(servoOutputY, DEC);
    Serial.print(F(", Spead = "));
    Serial.print(speedX, DEC);
    Serial.print(F(", "));
    Serial.println(speedY, DEC);
  }
}