Go Down

Topic: can virtual library be used with PID controller (Read 440 times) previous topic - next topic

adelphysi

Oct 07, 2017, 08:44 pm Last Edit: Oct 07, 2017, 09:07 pm by adelphysi
HI
i have been working one my cuad and i used  virtual library and timerservo2 library and wire library in one arduino uno but i  have got a result that D controller get some constant values in specific times and this reduces the efficiency of the controller unfortunately and made it jerky  ,i think if i have a faster arduino microcontroller i will achieve  balancing control system for the quad in one microcontroller than using standard  RC remote control with a controller .

any opinions?

here is my code :


Code: [Select]

#include <VirtualWire.h>

#include <ServoTimer2.h>

#include<Wire.h>

#define pinmyservo1 4
#define pinmyservo2 5
#define pinmyservo3 6
#define pinmyservo4 7

ServoTimer2 myservo1;    // declare variables for up to eight servos
ServoTimer2 myservo2;
ServoTimer2 myservo3;
ServoTimer2 myservo4;
int gyro_roll_cal ,gyro_pitch_cal,gyro_yaw_cal;
int esc_1, esc_2 ,esc_3, esc_4;
int  battery_voltage;
int start;
///////////////////////////////////////////////////////////
const int numberOfAnalogPins = 4; // how many analog integer values to receive
int data[numberOfAnalogPins]; // the data buffer
int value[numberOfAnalogPins];// the number of bytes in the data buffer

const int dataBytes = numberOfAnalogPins * sizeof(int);
byte msgLength = dataBytes;
/////////////////////////////////////////////////////////
float  gyro_roll_input,gyro_pitch_input,gyro_yaw_input;
float  gyro_roll,gyro_pitch,gyro_yaw;
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;


int throttle,pitch ,roll, yaw , pitch_control ,roll_control , yaw_control;
float X,Y,Z,Xp ,Xi ,Xd ,Yp ,Yi ,Yd ,Zp,Zi,Xl,Yl,roll_cal,pitch_cal,yaw_cal;  //pids
int cal_int ;
void setup(){

Serial.begin(9600);
 vw_set_ptt_inverted(true); // Required for DR3100
vw_setup(2500); // Bits per sec
vw_set_rx_pin(11);
vw_rx_start(); // Start the receiver
////////////////////////////////////////////////////
  Xi=0;
  X=0;
   Xl=0;
   Yl =0;
 
   Zi=0;
  

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
 /////////////////////////////////////////////

  myservo1.attach(pinmyservo1);     // attach a pin to the servos and they will start pulsing
  myservo2.attach(pinmyservo2);
  myservo3.attach(pinmyservo3);
  myservo4.attach(pinmyservo4);
   myservo1.write(1000);

   myservo2.write(1000);

   myservo3.write(1000);
 
    myservo4.write(1000);
    delay(6000);

     for (cal_int = 0; cal_int < 4000 ; cal_int ++){              //Take 2000 readings for calibration.
       //Change the led status to indicate calibration.    gyro_signalen();                                           //Read the gyro output.
    gyro_roll_cal += gyro_roll;                                //Ad roll value to gyro_roll_cal.
    gyro_pitch_cal +=gyro_pitch;                              //Ad pitch value to gyro_pitch_cal.
    gyro_yaw_cal += gyro_yaw;
  }
  gyro_roll_cal /= 4000;                                       //Divide the roll total by 2000.
  gyro_pitch_cal/= 4000;                                      //Divide the pitch total by 2000.
  gyro_yaw_cal /= 4000;  

start =0 ;
  }
     int incPulse(int val, int inc){
   if( val + inc  > 2000 )
 return 1000 ;
   else
   return val + inc;

}
void loop(){
  Serial.println(3*Xd);

      //int timer1 = micros();


   if( throttle< 1050 && yaw < 1050){start = 1;
    Xi=0;
   Xl=0;
   Xi=0;
   Yi=0;
   Yl=0;
   Zi=0;}
    if(start == 1 &&  throttle < 1050 && yaw > 1450){
   start = 2;
   Xi=0;
   Xl=0;
   Xi=0;
   Yi=0;
   Yl=0;
   Zi=0;
  }
  
 

 
 
  
    if(vw_get_message((byte*)data, &msgLength)) {

  if(msgLength == dataBytes){
for (int i = 0; i < numberOfAnalogPins; i++) {
//Serial.print("pin ");
//Serial.print(i);
//Serial.print("=");
//Serial.println(value[i]);
value[i]=map(data[i],0,1023,1000,2000); // Write into the servo

  throttle= value[0];
  roll=     value[1];
  pitch =   value[2];
  yaw =     value[3];
}}

 
  
  if(start == 2 && throttle < 1050 && yaw > 1900)
  {
  start = 0;

   esc_1=0;
    esc_2=0;
     esc_3=0;
      esc_4=0;
    
    

  }
  
 pid_cal ();
 
   battery_voltage = battery_voltage * 0.92 + (analogRead(0) + 65) * 0.09853;
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gyro_roll=Wire.read()<<8|Wire.read();
  if(cal_int == 4000) gyro_roll -= gyro_roll_cal;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
 gyro_pitch=Wire.read()<<8|Wire.read();
   if(cal_int == 4000) gyro_pitch -= gyro_pitch_cal;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
 gyro_yaw=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  if( cal_int == 4000) gyro_yaw -= gyro_yaw_cal;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)

    gyro_roll_input = ((gyro_roll_input * 0.7) + ((gyro_roll / 65.5) * 0.3));            //Gyro pid input is deg/sec.
  gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_pitch /65.5) * 0.3);         //Gyro pid input is deg/sec.
  gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_yaw / 65.5) * 0.3);               //Gyro pid input is deg/sec.
  /////////////////////////////////////

  if(throttle > 1800)throttle=1800;

esc_1 =throttle-pitch_control-roll_control +roll_cal+pitch_cal -yaw_cal;  //  throttle+ roll_cal-  pith_cal
esc_2 =throttle+pitch_control-roll_control +roll_cal-pitch_cal+yaw_cal;    //throttle+ roll_cal+  pith_cal
esc_3 =throttle+pitch_control+roll_control -roll_cal-pitch_cal -yaw_cal;    //throttle- roll_cal+  pith_cal
esc_4 =throttle-pitch_control+roll_control -roll_cal+pitch_cal+yaw_cal;    //throttle- roll_cal-  pith_cal
/////////////////////////////////////////////////
if( start != 1 ){
if (esc_1<1050)esc_1=1050;
  if (esc_2<1050)esc_2=1050;
   if (esc_3<1050)esc_3=1050;
   if (esc_4<1050)esc_4=1050;
}
else {
  esc_1=0;
    esc_2=0;
     esc_3=0;
      esc_4=0;
  }

roll_control =0;
  if (roll > 1510)roll_control=(roll-1510)/5;
  else if (roll < 1490)roll_control=(roll-1490)/5;
  
pitch_control =0;
   if ( pitch> 1510) pitch_control=( pitch-1510)/5;
  else if ( pitch < 1490) pitch_control=( pitch-1490)/5;
  
yaw_control=0;
if ( yaw> 1512)yaw_control=( yaw-1512)/8;
  else if ( yaw < 1488) yaw_control=(yaw-1488)/8;


 if (battery_voltage < 1240 && battery_voltage > 800){                   //Is the battery connected?
      esc_1 += esc_1 * ((1240 - battery_voltage)/(float)3500);              //Compensate the esc-1 pulse for voltage drop.
      esc_2 += esc_2 * ((1240 - battery_voltage)/(float)3500);              //Compensate the esc-2 pulse for voltage drop.
      esc_3 += esc_3 * ((1240 - battery_voltage)/(float)3500);              //Compensate the esc-3 pulse for voltage drop.
      esc_4 += esc_4 * ((1240 - battery_voltage)/(float)3500);              //Compensate the esc-4 pulse for voltage drop.
    }
  
      //int timer1 = micros();

   myservo1.write(esc_1);
   myservo2.write(0);
   myservo3.write(0);
   myservo4.write(esc_4);

 //int timer2 = micros();
//Serial.println("");
//Serial.println(esc_1);
//Serial.println(esc_2);
//Serial.println(esc_3);
//Serial.println(esc_4);

//Serial.println(timer2-timer1);
//Serial.println(pitch_cal);

//delay(100);
  


 
 
 


   }


}

void pid_cal(){
  //roll cal
 X=(gyro_roll_input)+6.37;//+6.37
 Xp=0.0*X;//0.8 propotional   roll
 Xi+=X;
 
 if(Xi>400)Xi=200;
 if(Xi<-400)Xi=-200;
 
 Xd=0.8*(X-Xl); //3derivative roll
  roll_cal =Xp+(0.0*Xi)+Xd;//0.007
 if(roll_cal>200)roll_cal=200;
 if(roll_cal<-200)roll_cal=-200;
  Xl=X ;

  //pitch cal
X=(((gyro_pitch_input)*-1)+0.23)*-1;
 Yp=0.0*X;  //0.01 propotional   pitch
 Yi+=X  ;  
 if(Yi>200)Yi=200;
 if(Yi<-200)Yi=-200;    
 Yd=0.0*(X-Yl); //3*derivative pitch

  pitch_cal =Yp+(0.0*Yi)+Yd;//0.007
  
 if(pitch_cal>200)pitch_cal=200;
 if(pitch_cal<-200)pitch_cal=-200;
 Yl=X ;

  
// yaw cal
Z=(gyro_yaw_input*-1)-7.7;//-7.7
Zi+=Z;
Zp=0*Z;//1.5 propotional   pitch

 
 yaw_cal =Zp+(0.0*Zi);//0.006
  if(yaw_cal>400)yaw_cal=400;
 if(yaw_cal<-400)yaw_cal=-400;

  
  }


hammy

#1
Oct 09, 2017, 01:01 pm Last Edit: Oct 09, 2017, 01:02 pm by hammy
i wouldn't use float variables in this , which might slow things down .
Have you tried tuning the PID loop ( starting with D off ).

Why are you not using a bought controller ( quad) for this , they're cheap have all the connectors and work , with great configuration software. They tend to use 32bit processors too - which might , as you imply, suggest the Arduino is too slow for this task , and the source of jitter.
?

adelphysi

i have made all floats to ints but no thing changes


the idea is mainly educational , and i dont prefer to buy some thing commercial the idea is to make it my self , arduino is realy has  a slow micro controller

i heard about stm32 it is 72mhz freq.

it is so fast and  can be programmed by Arduino IDE.
 


HermannSW

> it is so fast and  can be programmed by Arduino IDE.
>
Arduino Due has 84MHz (12$).
Lolin ESP32 with OLED has 160MHz (10$).
Both can be programmed with Arduino IDE.
https://forum.arduino.cc/index.php?topic=495555.0

bms001

Have you tried using the standard Servo library rather than ServoTimer2? It will use the 16bit Timer 1 rather than the 8bit timer 2. I don't know if this will make a difference or not but it could be worth a try (and it does not require changing your code much so easy to test - change servo.write() to servo.writeMicroseconds()).

Code: [Select]
   myservo1.write(esc_1);
   myservo2.write(0);
   myservo3.write(0);
   myservo4.write(esc_4);

What is this? Looks very strange to me.

adelphysi

stm32 is a 2$  arduino due 19 $


 
,  i cant use servo library with virtual wire  library because they both use the same timer (timer 1)

and when i did so there was a conflict  .

this is an under contraction  code , and i copied the post while i was working on it thats all  :)

so it is

Code: [Select]
   myservo1.write(esc_1);
   myservo2.write(esc_2);
   myservo3.write(esc_3);
   myservo4.write(esc_4);

HermannSW

> stm32 is a 2$  arduino due 19 $
>
You can get ESP32 dev boards without OLED for 5$ on aliexpress, 160MHz, Wifi and bluetooth already in place, and programmable by Arduino IDE:
https://github.com/espressif/arduino-esp32

adelphysi

> stm32 is a 2$  arduino due 19 $
>
You can get ESP32 dev boards without OLED for 5$ on aliexpress, 160MHz, Wifi and bluetooth already in place, and programmable by Arduino IDE:
https://github.com/espressif/arduino-esp32
yes it is about 7 $ < thank you my friend

MartinL

Hi adelphysi,

The Arduino Uno is more than capable of implementing a flight controller, even using float PIDs.

You just need to use the hardware I2C bus to the gyro on analog pins A4 and A5, preferably running at 400kHz.

You also need to use analogWrite() on digtial pins 3, 9, 10 and 11, as these run the PWM at a frequency of 490Hz, that's required to drive the ESCs. The 50Hz generated by the servo library too slow and in any case is software based so is prone to jitter.

Joop Brokking's videos on Youtube are a good starting point for any flight controller design.

adelphysi

#9
Oct 13, 2017, 12:42 pm Last Edit: Oct 13, 2017, 12:52 pm by adelphysi
Hi adelphysi,

The Arduino Uno is more than capable of implementing a flight controller, even using float PIDs.

You just need to use the hardware I2C bus to the gyro on analog pins A4 and A5, preferably running at 400kHz.

You also need to use analogWrite() on digtial pins 3, 9, 10 and 11, as these run the PWM at a frequency of 490Hz, that's required to drive the ESCs. The 50Hz generated by the servo library too slow and in any case is software based so is prone to jitter.

Joop Brokking's videos on Youtube are a good starting point for any flight controller design.
Hi MartinL

i  am already running at  400 kHz it is standardized int the i2c wire library

i have already tried the pins you mentioned ( it is known that they are PMW out put  on uno)

however it is not necessary to use them , i have tried that( as Joop Brooking says ^_^)


servo library is not slow but when i use virtual wire it becomes so slow (data show in excel sheet)


the code is based on Joop Brooking work , he is the godfather of drone deep understanding





 


bms001

The 50Hz generated by the servo library too slow and in any case is software based so is prone to jitter.
You can easily increase this by modifying this line
Code: [Select]
#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
in Servo.h

e.g. reduce to 10000 which equates to 100Hz.
How much you can increase this is limited by the need to fit 4 2000us pulses (plus some overhead) in this window.

Obviously if you have some conflict between servo and another library then this may make that worse. But on its own I found that making this change gave a significant improvement in terms of getting a quick and smooth response from a quadcopter.

i have already tried the pins you mentioned ( it is known that they are PMW out put  on uno)
however it is not necessary to use them , i have tried that( as Joop Brooking says ^_^)
How was it? Better/worse/no difference? What made you choose to use the 'servo' approach instead?

adelphysi

#11
Oct 13, 2017, 02:15 pm Last Edit: Oct 13, 2017, 02:52 pm by adelphysi
You can easily increase this by modifying this line
Code: [Select]
#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
in Servo.h

e.g. reduce to 10000 which equates to 100Hz.
How much you can increase this is limited by the need to fit 4 2000us pulses (plus some overhead) in this window.

Obviously if you have some conflict between servo and another library then this may make that worse. But on its own I found that making this change gave a significant improvement in terms of getting a quick and smooth response from a quadcopter.
How was it? Better/worse/no difference? What made you choose to use the 'servo' approach instead?
You can easily increase this by modifying this line
Code: [Select]
#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
in Servo.h

e.g. reduce to 10000 which equates to 100Hz.
How much you can increase this is limited by the need to fit 4 2000us pulses (plus some overhead) in this window.

Obviously if you have some conflict between servo and another library then this may make that worse. But on its own I found that making this change gave a significant improvement in terms of getting a quick and smooth response from a quadcopter.
How was it? Better/worse/no difference? What made you choose to use the 'servo' approach instead?
guys
i have tried servo at first but it made conflict with virtual wire so i use now servotimer2 instead

there was no deference   what ever i changed pins or change  data rate in virtual wire , using servo or  servotimer2 alone gave good results but when i just add the  virtual wire at the head of the code without even put any code related to it i got constant value intervals as i see in excel sheet which data is brought   from serial monitor of D controller values ,when i remove the library it goes fine


i have now used Leonardo board with virtual wire and software servo and there is no deference

 
 virtual wire slowers every thing
any opinion

bms001

It seems like the use of virtualwire has quite an impact on the rest of your design.

What exactly is it used for and do you have to use this library or are there other options?

adelphysi

simply i use it for making the flight controller and the RC receiver in one arduino module

because  i have built  RC remote , doing this is to show that we can build the drone with simple rf modules small  and few  components  instead of use of RC standard remote with its receiver

bms001

Why, exactly, do you need the VirtualWire library? What functionality does it support?

I assume that it is to communicate with the RF module - is that correct?
If that is the case then...
Which RF module are you using?
Is VirtualWire (or similar approach) the only option?
Is there any possibility of using an RF module which doesn't require VirtualWire e.g. the cheap and common nRF24L01+?

I'm not saying you have to get rid of VirtualWire (I don't even know if it actually the cause of your problems) - I am just trying to highlight other possibilities.

Go Up