Go Down

Topic: [UPDATES] Bicopter stabilization algorith (Code + Video) (Read 3981 times) previous topic - next topic

MartinL

Hi Marco,

Either way.

In my first flight controller I used getMotion6, as it's just an easier and cleaner way to get all the data from both the gyro and accelerometer.

rossi86m

#16
Mar 23, 2017, 03:07 pm Last Edit: Mar 23, 2017, 03:21 pm by rossi86m
Hi Marco,

Either way.

In my first flight controller I used getMotion6, as it's just an easier and cleaner way to get all the data from both the gyro and accelerometer.
Thanks

I will try both and see how it works.

Hopefully tomorrow i will be able to test this adjustments.

I attached both codes just in case anybody want to have a look at them and give me some advices.

I got rid of the aggressive kp ki and kd params

I will keep you posted.

Thanks!!!

rossi86m

#17
Mar 24, 2017, 06:42 pm Last Edit: Mar 25, 2017, 01:32 am by rossi86m
Hi Marco,

Either way.

In my first flight controller I used getMotion6, as it's just an easier and cleaner way to get all the data from both the gyro and accelerometer.
I am testing the values using
Code: [Select]

mpu.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);


And the values i am getting are crazy

And using analogwrite doen't make the motors move. I found this
Code: [Select]

analogWrite(escPin, High);
delay(1000);
analogWrite(escPin, Low);
delay(19000);

And that do not work neither

Any suggestions?

MartinL

Hi Marco,

Quote
And the values i am getting are crazy
What values are you getting from the getMotion6() function?

Quote
And using analogwrite doen't make the motors move.
I found the best way to approach this was to remove the props and write small sketch that feeds you throttle through to analogWrite() directly, without any roll, pitch, yaw, MPU or PID code. That way you'll know if it's working correctly or not.

The construction of a flight controller can really be broken down into a collection of mini projects:

1. Arm/Disarm Interlock
2. Receiver
3. Gyroscope
4. PIDs
5. Motor/Servo Mixing and Output

I found it much easier think about and solve each of these topics in isolation with number of small test sketches, before gradually bringing them together at the end.

rossi86m

Hi Marco,

What values are you getting from the getMotion6() function?

I found the best way to approach this was to remove the props and write small sketch that feeds you throttle through to analogWrite() directly, without any roll, pitch, yaw, MPU or PID code. That way you'll know if it's working correctly or not.

The construction of a flight controller can really be broken down into a collection of mini projects:

1. Arm/Disarm Interlock
2. Receiver
3. Gyroscope
4. PIDs
5. Motor/Servo Mixing and Output

I found it much easier think about and solve each of these topics in isolation with number of small test sketches, before gradually bringing them together at the end.
I am creating separate sketches to test mpu, bluetooth, and motors.

I am having a hard time figuring out how to control the brushless motors using analogwrite. Do you have a short example on how to arm them and how to power them?

Thanks,
Marco

rossi86m

Update:

I clean up the code and separate it into functions.

I was able to combine MPU, PID for servos and BT connection.

I am still having a hard time arming and operating brushless motors using analogwrite.

I wasn't able to calculate angles using mpu.getmotion6. I was getting a nan output after doing this math

Code: [Select]

Angle[0] = 0.98 * (Angle[0] + Gy[0] * 0.010) + 0.02 * Acc[0];
Angle[1] = 0.98 * (Angle[1] + Gy[1] * 0.010) + 0.02 * Acc[1];

Gy[] and Acc[] values were ok, so i do not know was what going on.

But when getting values from mpu using wire it worked.

I attach my new code. Still trying to figure out how to work with analogwrite for brushelss.

Any suggenstion or sample code????

MartinL

#21
Mar 26, 2017, 06:33 pm Last Edit: Mar 26, 2017, 06:37 pm by MartinL
Hi Marco

The other option is to use hardware PWM rather than analogWrite.

The following code will control your two motors with 400Hz PWM signals, at 11-bit resolution on D9 and D10:

Code: [Select]
void setup() {
  // Initialise timer 1 for phase and frequency correct PWM
  pinMode(9, OUTPUT);                         // Set digital pin 9 (D9) to an output
  pinMode(10, OUTPUT);                        // Set digital pin 10 (D10) to an output
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);         // Enable the PWM outputs OC1A, and OC1B on digital pins 9, 10
  TCCR1B = _BV(WGM13) | _BV(CS11);            // Set phase and frequency correct PWM and prescaler of 8 on timer 1
  ICR1 = 2500;                                // Set the PWM frequency to 400Hz
  OCR1A = 0;                                  // Set motor throttle to 0 on D9
  OCR1B = 0;                                  // Set motor throttle to 0 on D10
}

void loop() {
  OCR1A = 1000;                               // Set motor throttle to 0 on D9
  OCR1B = 1000;                               // Set motor throttle to 0 on D10
  delay(1000);                                // Wait for 1 second
  OCR1A = 1200;                               // Set motor throttle to low on D9
  OCR1B = 1200;                               // Set motor throttle to low on D10
  delay(1000);                                // Wait for 1 second
}

Just load the OCR1A or the OCR1B registers with the value in microseconds you'd like to send to each ESC. The exact values will depend on your ESC calbration, but are usually somewhere between 1000us (standby) and 2000us (maximum throttle).

Note that in this instance, there's no need to map your values between 0 and 255 like analogWrite().

rossi86m

#22
Apr 01, 2017, 08:28 pm Last Edit: Apr 01, 2017, 09:32 pm by rossi86m
Hi Marco

The other option is to use hardware PWM rather than analogWrite.

The following code will control your two motors with 400Hz PWM signals, at 11-bit resolution on D9 and D10:

Code: [Select]
void setup() {
  // Initialise timer 1 for phase and frequency correct PWM
  pinMode(9, OUTPUT);                         // Set digital pin 9 (D9) to an output
  pinMode(10, OUTPUT);                        // Set digital pin 10 (D10) to an output
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);         // Enable the PWM outputs OC1A, and OC1B on digital pins 9, 10
  TCCR1B = _BV(WGM13) | _BV(CS11);            // Set phase and frequency correct PWM and prescaler of 8 on timer 1
  ICR1 = 2500;                                // Set the PWM frequency to 400Hz
  OCR1A = 0;                                  // Set motor throttle to 0 on D9
  OCR1B = 0;                                  // Set motor throttle to 0 on D10
}

void loop() {
  OCR1A = 1000;                               // Set motor throttle to 0 on D9
  OCR1B = 1000;                               // Set motor throttle to 0 on D10
  delay(1000);                                // Wait for 1 second
  OCR1A = 1200;                               // Set motor throttle to low on D9
  OCR1B = 1200;                               // Set motor throttle to low on D10
  delay(1000);                                // Wait for 1 second
}

Just load the OCR1A or the OCR1B registers with the value in microseconds you'd like to send to each ESC. The exact values will depend on your ESC calbration, but are usually somewhere between 1000us (standby) and 2000us (maximum throttle).

Note that in this instance, there's no need to map your values between 0 and 255 like analogWrite().
I am testing what you suggested and the code alone works perfect but when i integrate it with the whole bicopter code the servos on pin 6 and 5 stop working.

If i comment the initServos (); On the setup the motors work. I think that is because servo.h use timer1 but i do not know to much about timers

Code attached pls help,

Looking foward to hearing your comments,
Marco

MartinL

#23
Apr 03, 2017, 09:54 am Last Edit: Apr 03, 2017, 11:17 am by MartinL
Hi Marco,

Quote
I think that is because servo.h use timer1...
Yes, you're right. I overlooked the fact that the servo library uses timer1. This is also the reason why analogWrite() on pins 9 and 11 (which also uses timer1) didn't work.

To overcome this it's necessary to use the 8-bit timer2 for your motors and the 16-bit timer1 for your servos. This is because only timer1 is capable of phase and frequency correct PWM that allows you to set the frequency at 50Hz for your servos.

Timer2 on the other hand can only operate in phase correct PWM, which can be set at 490Hz that's suitable for your motor ESCs.

To convert timer1 for servo operation at 50Hz just change the ICR1 register from 2500 to 20000. Load the OCR1A and OCR1B registers with 1500 to centre the servos. Here's the code, it's identical to the previous code except that I've changed the ICR1 register to 20000:

Code: [Select]
void setup() {
  // Initialise timer 1 for phase and frequency correct PWM at 50Hz on digital pins D9 and D10
  pinMode(9, OUTPUT);                         // Set digital pin 9 (D9) to an output
  pinMode(10, OUTPUT);                        // Set digital pin 10 (D10) to an output
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);         // Enable the PWM outputs OC1A, and OC1B on digital pins 9, 10
  TCCR1B = _BV(WGM13) | _BV(CS11);            // Set phase and frequency correct PWM and prescaler of 8 on timer 1
  ICR1 = 20000;                               // Set the PWM frequency to 50Hz
  OCR1A = 1500;                               // Centre the servo on D9 and D10
  OCR1B = 1500;                             
}

void loop() {
  OCR1A = 1300;                               // Move the servo on D9 and D10
  OCR1B = 1300;                               
  delay(1000);                                // Wait for 1 second
  OCR1A = 1700;                               // Move the servo on D9 and d10
  OCR1B = 1700;                               
  delay(1000);                                // Wait for 1 second
}

For the your motors, either use analogWrite() function on digital pins D3 and D11, or use the following code that sets up timer2 for phase correct PWM at 490Hz on D3 and D11:

Code: [Select]
// Set up timer 2 to output 490Hz, 8-bit resolution PWM on pins D11 and D3
void setup() {
  pinMode(11, OUTPUT);                                // Set digital pin 11 (D11) to an output (OC2A)
  pinMode(3, OUTPUT);                                 // Set digital pin 3 (D3) to an output (OC2B)
  TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);    // Enable phase correct PWM outputs OC2A and OC2B on digital pins D11 and D3
  TCCR2B = _BV(CS22);                                 // Set the prescaler to 64 on timer 2
}

void loop() { 
  OCR2A = 125;         // Set motor ESCs to standby at 1000us pulsewidth
  OCR2B = 125;                             
  delay(2000);         // Wait for 2 seconds
  OCR2A = 150;         // Set motor ESCs to idle up at 1200us pulsewidth
  OCR2B = 150;                             
  delay(2000);         // Wait for 2 seconds
}

Note that for the motors you'll have to map your 0us to 2000us ESC pulses to 0 to 255, as timer2's only 8-bit.

The reason for all this, is the fact that the Arduino Nano's Atmel 328P microcontroller has only one 16-bit timer (timer1) with 2 output channels.

Alternatively, you could consider the Arduino Micro with its Atmel 32U4 microcontroller. This has two 16-bit timers (timers1 & 3) with 4 output channels and would give you higher a 11-bit resolution PWM for your motors, in addition to your servos. (The disadvantage of the Arduino Micro is that it only has a limited number of interrupt pins).

That being said, the Arduino Nano's been successfully used in a lot of multi-rotor projects.

rossi86m

Quote
For the your motors, either use analogWrite() function on digital pins D3 and D11, or use the following code that sets up timer2 for phase correct PWM at 490Hz on D3 and D11:
Thanks for the answer i really appreciate it. Right now i am using pin 2 and 3 for the hc-05 since i am using softwarserial

I think i can use any of the digital pins for that right?

So in my case i will use:
- Pins 9 and 10 -> servos
- Pins 3 and 11 -> motors
- Pins 5 and 6   -> hc05

Is this ok?

Thanks,
Marco

MartinL

Hi Marco,

Quote
So in my case i will use:
- Pins 9 and 10 -> servos
- Pins 3 and 11 -> motors
- Pins 5 and 6   -> hc05

Is this ok?
Yes. The software serial library should work just fine with digital pins 5 and 6.


rossi86m

Hi Marco,

Yes. The software serial library should work just fine with digital pins 5 and 6.


Hey good news. The three pieces are working great and without interferences!!!!!!
Motors
Servos
Bluetooth

Now i have to seat down and tune pid values :)

I attach my code just in case someone wants to check it out

Martin thanks for the advice

More updates/questions/good news. Coming soon

Cheers

rossi86m

#27
Apr 07, 2017, 04:02 pm Last Edit: Apr 07, 2017, 05:30 pm by rossi86m
Does someone has any suggestion on how to tune the pid values that controls the servos?

With the ones that controls the brushless is easier because by holding the bicopter and move it. if it goes back to its position properly is ok

But with servos i do not know how to test if the value are tune properly

I also attach the android app that i developed for this

Looking forward to hearing suggestions

Thanks,
Marco

MartinL

#28
Apr 08, 2017, 10:40 am Last Edit: Apr 08, 2017, 10:48 am by MartinL
Hi Marco,

To find the correct PID values, I'd suggest starting off by initially only using the proportional or P term. You should be able to get airborne using the P term alone. Then when you're happy with results then gradually increase the I term, followed lastly by the D term.

The correct gain values will be determined by the value of your inputs, for example inputs in degrees/s will have a different gain from say inputs in radians/s. Another factor is the size of your aircraft. Usually the larger the aircraft the greater the gain.

I'm using roll, pitch and yaw rate control loops using degrees/s (for both the pilot setpoint and the gyro input). Here are the PID values for for my small 250mm tricopter:

Roll
P: 0.8
I: 0.5
D: 0.0002

Pitch
P: 0.8
I: 0.5
D: 0.0002

Yaw (Servo Controlled)
P: 1.6
I: 0.2
D 0.0

Obviously these values are just an example, as your bicopter is a different configuration (especially the pitch axis, which uses servo rather than motor control), but the priciple is the same. I'd start off using lower values P gain values first then gradually increase them until the aircraft becomes more responsive, before adding I and D.

rossi86m

It still doesn't fly yet but i am close to achieve that.

One of my main obstacles now is how to control the yaw.

I know that i should add the yaw correction on the servos positions but what i can figure out yet is how to calculate the yaw using the mpu6050.

I read some articules that say that you need a digital compass but in some other places say that the mpu6050 can do it.

Any suggestion on this?

Thanks,
Marco

Go Up