Trouble with servos and passing arrays

Projects Programming
1

I have been having problems with my code. It should be pretty simple. I am using a 4 servo arm to move with a FlySky transmitter. I have added some filters but to do this I kept all the servo settings in different an array passed to a function. Compiles just fine, and but when used on the final object, it acts on it's own. It moves forward w/o any input and rejects any input calls from the transmitter. It's not a complicated code, but I just can't find my errors. Any help would be greatly appreciated. First time posting her and crossing my fingers.

2

Sorry I thought I had to add my code later, but here it is.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// Servo Controls through driver
#define Servo_min 140
#define Servo_max 610
#define midValue 377
const int increment = (Servo_max - Servo_min)/180;
const int Freq = 50;
const int servo_freq = 10;

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// 0: Servo_Channel 1: servo_timestamp 2: Current_Angle 3: Previous_Angle
int Servo1[5] = {0, 0, 0, 0};
int Servo2[5] = {1, 0, 0, 0};
int Servo3[5] = {2, 0, 0, 0};
int Servo4[5] = {3, 0, 0, 0};


// FlySky Inputs: only using CH3
#define CH1 3 //Left-Right
#define CH2 5 //Forward-Reverse
#define CH3 6 //Throttle
#define CH4 9 //LR
#define CH5 10 //Switch

//Debug Light
#define LED 2

int ch1input;
int ch2input;
int ch3input;
int ch4input;
bool ch5input;


// Function to read RC inputs
int readChannel(int channelInput, int minLimit, int maxLimit, int defaultValue){
  int ch = pulseIn(channelInput, HIGH, 30000);
  if (ch < 100) return defaultValue;
  return map(ch, 1000, 2000, minLimit, maxLimit);
}

// Function to read switch input
bool readSwitch(byte channelInput, bool defaultValue){
  int intDefaultValue = (defaultValue)? 100: 0;
  int ch = readChannel(channelInput, 0, 100, intDefaultValue);
  return (ch > 50);
}

// Servo[i][5] : Servo# 0, Servo timestamp 1, current angle 2, previous angle 3
void my_moveServo(int setAngle, int Servo[5]){ //Pass Channel Read, Servo, and buffer
  int temp[5];
  for (int i = 0; i < 5; i++) temp[i] = Servo[i]; //Set the buffer to the Servo array
  double currentMills = millis();
  if ((currentMills - Servo[1]) > servo_freq){ //Is the servo ready to move
    temp[1] = currentMills;

    if (abs(setAngle - Servo[3]) < 5) setAngle = Servo[3]; //Has the angle moved enough
    if (setAngle > Servo[2]) temp[2] += increment; // Start moving the Servo this amount
    if (setAngle < Servo[2]) temp[2] -= increment;
    pwm.setPWM(Servo[0], 0, temp[2]);// Actually move servo
    temp[3] = setAngle; //Last channel read value to prevent excess movement
    for (int i = 0; i < 5; i++) Servo[i] = temp[i]; // Update servo array with filtered movements
  }
}


void setup() {

  Serial.begin(9600);

  // FlySky Inputs
  pinMode(CH1, INPUT);
  pinMode(CH2, INPUT);
  pinMode(CH3, INPUT);
  pinMode(CH4, INPUT);
  pinMode(CH5, INPUT);

  //LED Debug
  pinMode(LED, OUTPUT);

  bool kill = pwm.begin();
  if (!kill){
    while (1) digitalWrite(LED, HIGH);
  }
  pwm.setPWMFreq(Freq);

  pwm.setPWM(Servo1[0], 0, Servo_min);
  pwm.setPWM(Servo2[0], 0, Servo_min);
  pwm.setPWM(Servo3[0], 0, Servo_min);
  pwm.setPWM(Servo4[0], 0, Servo_min);

  for (int x = 0; x <= 5; x++){
    digitalWrite(LED, HIGH);
    delay(100);
    digitalWrite(LED, LOW);
  }
  digitalWrite(LED, LOW);
  
}

void loop() {
  
  ch1input = readChannel(CH1, 0, 255, 0);
  ch2input = readChannel(CH2, 0, 255, 0);
  ch3input = readChannel(CH3, 0, 255, 0);
  ch4input = readChannel(CH4, 0, 255, 0);
  ch5input = readSwitch(CH5, false);

  int pulse[4] = {0, 1, 2, 3};

  pulse[0] = map(ch1input, 0, 255, Servo_min, Servo_max);
  pulse[1] = map(ch2input, 0, 255, Servo_min, Servo_max);
  pulse[2] = map(ch3input, 0, 255, Servo_min, Servo_max);
  pulse[3] = map(ch4input, 0, 255, Servo_min, Servo_max);

  my_moveServo(pulse[0], Servo1);
  my_moveServo(pulse[1], Servo2);
  my_moveServo(pulse[2], Servo3);
  my_moveServo(pulse[3], Servo4);
}
3

Verify the Flysky output at the Flysky, and again at the Arduino input.

4

add some Serial.print() to see what your sketch is doing and what values you variables contain.

5

I don't know what value you expect increment to have. If you do the division using floats, or on a calculator, the answer would be 2.61r. Doing the division with integers, as in your code, will set increment to 2.

6

I agree. I had thrown that in there to change the difference from adding a simple +1 to servo.move and thought i could flush out a number to change the PWM rate. It was something I am going to narrow in on once I get the overall code to run as expected.

7

I did run a simple scheme of turning off the FlySky remote and letting it run, and it does stop with the remote off. Problem is I already flushed out that code and got it running great before I got to this step. Though I will redo it from the start as nothing else is working

8

I've taken them out here for simplification. It is really messy where I have them now during the debug phase.

9

When one device needs information from another device... always measure (or print) the value so you know the right stuff is being sent and received.

10

That is not how to work with time on Arduino.

I also saw pulseIn() used in readChannel()... it blocks execution and can lead to nasty problems.

You should learn proper use of millis() or micros() timing and asynchronous timing code to coordinate the movements. The easier lessons have to do with leds and buttons to learn the techniques behind non-blocking event-driven code.

11

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.