micro airplane servo driver

Using Arduino Motors, Mechanics, Power and CNC
#1

Hello I am Ivan from Italy.I have make a little servo for micro airplane.But i have a problem.This servo use Pid control and take the signal from a rx(1000-2000ms).I think the problem is in pulse conversion for drive the motor. i don’t know how i can solve.Can you help me?I don’t know how convert the pulse from receiver in a set-servo-position!Thanks!

//servo driver//

#include <PinChangeInt.h>

int entrata = 8;
volatile int pulse = 0;
volatile int prev_time = 0;
uint8_t latest_interrupted_pin;
int timeChange;
float kp, kd;
float SampleTime = 0.001;
unsigned long lastTime;
float Input0,  Output0,  Set0;
float dErr0, error0, lastErr0;
unsigned long preTime = 0;
 

void rising()
{
 latest_interrupted_pin = PCintPort::arduinoPin;
 PCintPort::attachInterrupt(latest_interrupted_pin, &falling, FALLING);
 prev_time = micros();
}

void falling() {
 latest_interrupted_pin = PCintPort::arduinoPin;
 PCintPort::attachInterrupt(latest_interrupted_pin, &rising, RISING);
 pulse = micros() - prev_time;
 //Serial.println(pulse);
}

void setup()  {
 PCintPort::attachInterrupt(entrata, &rising, RISING);
 pinMode(7, OUTPUT);
 pinMode(9, OUTPUT);
 //Serial.begin(9600);
 //delay(100);

}

void loop() {
 PIDSERVO();
 PWMSC();
}
void  PIDSERVO() {
 kp = 10;
 kd = 10;
 unsigned long now = millis();
 timeChange = (now - lastTime);
 if (timeChange >= SampleTime)
 {
  
 pulse = map (pulse, 1000, 2000, 200,700);//  I think the problem is here!
   Set0 = pulse;
   Serial.print(Set0);
   Serial.print("  ");
   delay(100);
  Input0 = analogRead(A1);
   Serial.print(Input0);
   Serial.print("  ");
   delay(100);
   error0 = Set0 - Input0;
  dErr0 = ( error0 - lastErr0 ) / timeChange;
  Output0 = kp * error0  + kd * dErr0;
 if (Output0 > 255) {
    Output0 = 255;
  }
  else if (Output0 < -255)    {
     Output0 = -255;
   }
  lastErr0 = error0;
  lastTime = now;
}
}
void PWMSC() {
if (Output0 < 0)
{
  digitalWrite(7, LOW);
  analogWrite(9, Output0);
}
 else if (Output0 > 0)
 {
   digitalWrite(7, HIGH);
   analogWrite(9, Output0);
 }
else
 {
  digitalWrite(7, 0);
  digitalWrite(9, 0);
}
}
#2

You need to attach a single interrupt routine with CHANGE, not RISING or FALLING. There is
only one ISR per pin (normal interrupts) or per port (pin-change interrupts). You cannot have
separate ISRs for rising and falling.

#3

Thanks MikeT.I have make the change and now the servo work well.I need to add a little dead-band in the middle so the servo don’t jitter.Can you help me?I attach the new code that i have tested and work great.

//SERVO DRIVER CON PID E LETTURA SEGNALE DA RICEVENTE PWM-Pepe Ivan-2016//
//V.1//
#define RICEVENTE_SEGNALE_IN 1 // INTERRUPT 1 = DIGITAL PIN 2 - use the interrupt number in attachInterrupt
#define RICEVENTE_SEGNALE_IN_PIN 2 // INTERRUPT 0 = DIGITAL PIN 2 - use the PIN number in digitalRead
#define NEUTRAL_THROTTLE 1500 // this is the duration in microseconds of neutral throttle on an electric RC Car
#define MOTOR1 7
#define MOTOR2 9

int timeChange;
float kp, kd;
float SampleTime = 0.001;
unsigned long lastTime;
float Input0,  Output0,  Set0;
float dErr0, error0, lastErr0;
unsigned long preTime = 0;
int Servo_data[1];

volatile int pulse = NEUTRAL_THROTTLE; // volatile, we set this in the Interrupt and read it in loop so it must be declared volatile
volatile unsigned long ulStartPeriod = 0; // set in the interrupt
volatile boolean bNewThrottleSignal = false; // set in the interrupt and read in the loop

void setup()
{
  pinMode(MOTOR1, OUTPUT);
  pinMode(MOTOR2, OUTPUT);
  attachInterrupt(RICEVENTE_SEGNALE_IN,calcInput,CHANGE);
  //Serial.begin(9600);
}

void loop()
{
 if(bNewThrottleSignal)
 {

   //Serial.println(pulse); 

   bNewThrottleSignal = false;
 }
   {
  PIDSERVO();
  PWMSC();
   }
 }
 void  PIDSERVO() {
  kp = 0.25;
  kd = 1.2;
  unsigned long now = millis();
  timeChange = (now - lastTime);
  if (timeChange >= SampleTime)
  {
     Servo_data[1] = map(pulse, 1000, 2000, 240, 730);
     Set0 = Servo_data[1];
    //Serial.print(Set0);
    // Serial.print("  ");
    //delay(100);
    Input0 = analogRead(A1);
    //Serial.print(Input0);
    //Serial.print("  ");
    // delay(100);
    error0 = Set0 - Input0;
    dErr0 = ( error0 - lastErr0 ) / timeChange;
    Output0 = kp * error0  + kd * dErr0;
    if (Output0 > 255) {
      Output0 = 255;
    }
    else if (Output0 < -255)    {
      Output0 = -255;
    }
    lastErr0 = error0;
    lastTime = now;
  }
}
void PWMSC() {
  if (Output0 < 0)
  {
    digitalWrite(7, LOW);
    analogWrite(9, Output0);
  }
  else if (Output0 > 0)
  {
    digitalWrite(7, HIGH);
    analogWrite(9, Output0);
  }
  else
  {
    digitalWrite(7, 0);
    digitalWrite(9, 0);
  }
}


void calcInput()
{
  if(digitalRead(RICEVENTE_SEGNALE_IN_PIN) == HIGH)
  {
    ulStartPeriod = micros();
    
  }
  else
  {
    if(ulStartPeriod && (bNewThrottleSignal == false))
    {
      pulse = (int)(micros() - ulStartPeriod);
      ulStartPeriod = 0;
      bNewThrottleSignal = true;
      
    }
  }
}
#4

Add a new variable to use as the output, but only set it to the value of Output0 if its more than
deadband units different:

  if (abs (Output0 - Output0_filtered) >= DEADBAND)
    Output0_filtered = Output0 ;

then jitter of less than DEADBAND will be filtered out.

Who's MikeT?

#5