Post Reply 
 
Thread Rating:
  • 0 Votes - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Change MPU6050 Orientation In The Program
11-28-2016, 09:38 AM (This post was last modified: 11-28-2016 09:39 AM by ewo.)
Post: #1
Change MPU6050 Orientation In The Program
I rotate my MPU6050 sensor 90 degree prior to the instruction. So what part of the program should I change?

Is it this part?

Code:
float dmpGetPhi() {
  mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.resetFIFO();  // We always reset FIFO

  //return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
  //return Phi angle (robot orientation) from quaternion DMP output
  return (atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);
}

How to change it?

It will be hard to change the sensor orientation because I have already made the mounting PCB. I hope anybody can help.

Thanks.
Find all posts by this user
Quote this message in a reply
11-28-2016, 09:56 AM
Post: #2
RE: Change MPU6050 Orientation In The Program
Yes, you are right ->You will pass to use the ROLL info instead the regular TILT. So, modify the code to something like this (comment/uncomment):

return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
//return Phi angle (robot orientation) from quaternion DMP output
//return (atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);

* Check the sign of the "returned" roll value according to your setup
Visit this user's website Find all posts by this user
Quote this message in a reply
11-29-2016, 02:31 AM (This post was last modified: 11-29-2016 06:09 AM by ewo.)
Post: #3
RE: Change MPU6050 Orientation In The Program
(11-28-2016 09:56 AM)jpedroc Wrote:  Yes, you are right ->You will pass to use the ROLL info instead the regular TILT. So, modify the code to something like this (comment/uncomment):

return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
//return Phi angle (robot orientation) from quaternion DMP output
//return (atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);

* Check the sign of the "returned" roll value according to your setup

Thank you very much! I will try it and let's see the result.

Update:

Hi, I've changed the code and it works. However, only my first motor spins. My second motor does not spin at all. I've tried to run a simple code to test the second motor and it works. So it's not a hardware failure.

FYI, I changed the second motor connection to Arduino as follows:


Second motor
Step pin - 6
Dir pin - 5

So I change the Brobot code to:

// We generate 1us STEP pulse
SET(PORTD, 7); // STEP MOTOR 2
delay_1us();
CLR(PORTD, 7);

and

pinMode(6, OUTPUT); // STEP MOTOR 2 PORTD,7


The second problem is when the robot is in steady position (I hook the robot on a holder), The first motor runs in one direction. It does not stabilize. Is it the PID or some initial parameters that I should change? Could you guide me?

The third problem is, the response of the motor is too slow. What did I do wrong?

I really need your help. Thank you.
Find all posts by this user
Quote this message in a reply
11-29-2016, 10:15 AM
Post: #4
RE: Change MPU6050 Orientation In The Program
Did the motors work fine when the IMU and the code are set by default? (not modifying anything)

Have you tried to play with the Brobot with the regular settings?

We should narrow down the problem checking that before changing things....
Visit this user's website Find all posts by this user
Quote this message in a reply
11-29-2016, 12:01 PM
Post: #5
RE: Change MPU6050 Orientation In The Program
(11-29-2016 10:15 AM)jpedroc Wrote:  Did the motors work fine when the IMU and the code are set by default? (not modifying anything)

Have you tried to play with the Brobot with the regular settings?

We should narrow down the problem checking that before changing things....


I haven't tried it because I have already made a PCB and connects all of them. So, I need to disconnect all of the modules first. I will let you know the result later.

Please stay in this thread and help me. Big Grin
Find all posts by this user
Quote this message in a reply
11-30-2016, 04:52 AM (This post was last modified: 11-30-2016 07:23 AM by ewo.)
Post: #6
RE: Change MPU6050 Orientation In The Program
(11-29-2016 10:15 AM)jpedroc Wrote:  Did the motors work fine when the IMU and the code are set by default? (not modifying anything)

Have you tried to play with the Brobot with the regular settings?

We should narrow down the problem checking that before changing things....

Hi, it turns out that the connections of the PCB are not well established. I have tried it on a prototype board and both of the motors work.

Now the next problem is, when the sensor in steady position, the motors keep running. The robot seems not having an initial reference for a stable position. Then when I put on the ground, the motor seems not strong enough to balance. I use Nema 17 (17HS4401) which has 40Ncm holding torque. How do I overcome this?

And also, the response of the motor is improved, but not quite fast.

What's the next thing to do? Maybe it's due to the PID. How to tune the PID?
Find all posts by this user
Quote this message in a reply
11-30-2016, 11:11 AM
Post: #7
RE: Change MPU6050 Orientation In The Program
"What's the next thing to do? Maybe it's due to the PID. How to tune the PID?"

Well, I guess you will have to "try and fail"!, as far as I know, the official JJRobots Android APP has a way to tune the PID. Try that
Find all posts by this user
Quote this message in a reply
12-01-2016, 07:34 AM
Post: #8
RE: Change MPU6050 Orientation In The Program
(11-30-2016 11:11 AM)JohnQ Wrote:  "What's the next thing to do? Maybe it's due to the PID. How to tune the PID?"

Well, I guess you will have to "try and fail"!, as far as I know, the official JJRobots Android APP has a way to tune the PID. Try that

I will go to the PID tuning later. The problem now is the motors are stalling. Does the current limit cause it? The motor has current limit of 1.7 A per phase. And I use A4988 stepstick driver. It has Rs of 0.1 Ohm. So I calculate the Vref to limit the current to 1.5A:

Vref=1.5A*8*0.1 Ohm= 1.2 V

But the motors still stall. How do I correctly set the current limit?
Find all posts by this user
Quote this message in a reply
Post Reply 


Forum Jump:


User(s) browsing this thread: 1 Guest(s)