Well, here's a time-line of problems to give you some background. I'm building a balancing robot using an Arduino Uno and a 6050, with the on-board DMP giving me pitch data. Also, I'm using 1:30 37D Motors from Polulo with their 90mm wheels, driven by one of those square, generic 2A H-bridges with the heatsink on top.
Long story short, I put in the timing loop from Kaz's "Balancing Robots for Dummies" (see code), but it made the gyro act up and give off huge amount of noise intermittently, but frequently. Generally, more noise than data, there' no FIFO overflows.
Take out loop, and they gyro settles down, but the wheels don't change direction. Eventually get it so that when it goes in one direction, it's fine but in the other it's noisy and jerky. Debugging reveals that when it's choosing the if direction, it goes to the else selection every 5 loops.
So, here's the thing. Despite being supposedly essential for a balancing robot's PID, the PID works fine without it. The H-Bridge is getting the appropriate PWM signal for the angle coming from the DMP, it's just that it's choosing the wrong direction on a regular basis. But, I have no idea why.
I've attached the code for inspection, I hope somebody picks up something I missed.
Probably just need to go simple first. Like, get (log) the sensor readings....then manually tilt the bot forward and backward to see if the driving algorithm etc is behaving properly. Make sure everything is calibrated (sensor readings) and fairly stable. This is done with no power going to the wheels. Just observe sensor readings on computer screen (via serial communications).
So use the simplest code possible for testing. Eg.... if angle less than or equal to XX, output a value of -1. If angle greater than XX, output a value of +1.
I took your advice and commented out everything except the getPitch() and "forward/backward" decision, which I kept in the loop() and reduced to just printing out Forwards or Backwards. When it's just that, it works fine. But, put anything else back in, including using the goForward/Backwards() functions for just a Serial.println (digital.Write, et al commented out) I get two forwards for every backwards when pointing forwards. Adding back the readPots() didn't change anything. Even adding the writing PWM values to the motor control without the PID function didn't have any effect. But when I added the PID function (everything back to normal) it went back to four Forwards for every Backwards.
But, When I put everything back in, but had the direction decision just be to print "Forward" or "Backward" (no functions), everything worked fine. But, put anything back, even just convert the negative pitch values (which designate forward) to abs() so I can send them to PID, it went back to 4 Forwards, 1 Backwards. I tried moving the abs() calculation to right before Compute.PID in loop() to make the "if" and "else" decisions be more symmetrical, but that did nothing. Same for increasing the Serial Port to 230400 baud, which had no effect.
If I add various stuff back randomly, I can get other patterns of Forward/Backwards including 1 forward, 2 backwards and a few that aren't regular. So, it appears that simply adding or commenting out lines of code will effect this even if the direction decision isn't involved.
I've had a few instances of the code working for a few seconds, then falling back to the reversing direction problem. But, I'm not sure what that's about.
Nice work in the testing of the simple system. This is good. Make sure to check your 'if' statements. Also, run through your initialised values.... such as initial value of lastLoopUsefulTime, which is '9' millisecond.
The first 'if' statement will give a 'not true' result (see below).... since it will be if (9 < 9). However, probably need to check your 'if' structure. At the moment, you have:
But note....as it currently stands, your first 'if' statement will be 'not true', so you might consider using <= instead of a < symbol.
But also note - in your original code that you posted, it is all commented out with /* */
So it appears that loopStartTime etc are never updated, since they are all commented out.
I commented out the loop because when it was running, my gyro kept giving me extremely noisy, unusable data. Also, the serialOut_timing() function sent back data where lastLoopTIme occasionally changed from 9 to 10 (sometimes 11), while lastLoopUsefulTime varied between 3 and 7, but I'm not sure what that means as Kaz never covered what to do if you don't get the 2,10 data he said was normal.
However, when I replaced it with your version of the timing loop, the serialOut_time() function returned a lastLoopUsefulTime that kept increasing, starting at 832 and increasing by about 27-28 each time. So, since it skips over the whole loop when lastLoopUsefulTime !< STD_LOOP_TIME, it's probably just bypassing the loop each time through and not keeping the 100Hz timing rate. However, the gyro worked fine, because no timing loop, no gyro problems. I moved the bracket up a line and while it seemed to work (at least send back more valid looking data), the gyro noise started up again.
Note: I left in the lastLoopUsefulTime = millis()-loopStartTime; that goes before the if statement even though it's not in your code because without it, lastLoopUsefulTime never updates and stays at 9.
I did change my if statement in direction selection to Gyro_Pitch <= 0 instead of just a < and discovered as long as I don't convert the data to its absolute value (either by abs() or by * -1), which is necessary for the PID to read it, I no longer have the direction problem. It's just that the PID will send 0 when it leans forward instead of regularly switching direction at the right speed. However, even if I do get it to work by finding the right number of lines of code, I won't be able to get my robot to move around as anything I do will just destabilize it. So, I don't think this will be a good long-term strategy.
I see. Let me recommend using the accelerometer measurements from the mpu6050 to obtain tilt angles to begin with. Ignore gyro readings for the moment, and use calibrated accelerometer values to calculate bot tilt angle.
This should give you a nice start in working toward good balancing action. The angle calculations come from a ratio of say x axis and z axis accelerometer readings. You probably know how to obtain the angle already.
The control algorithm can look at the deviation in angle (relative to the upright 90 degree value). And the motors can be driven in directions to minimise the deviation in angle.... which will mainly involve applying Kp to the deviation angle; and Kd to the time incremental change in the deviation angle. Then at the... can also apply Ki to the accumulated sum of the time incremental changes in angle deviation...... a little bit of Ki only.
The incremental time change.... aka sampling period can be say microsecond time scales.
The very first parameter to set is Kp via potentiomerer. The other two are usually set to zero until the bot tries to balance (even if it has seesaw action). Then Kd will usually dampen the wobbles. Then a touch of Ki helps to reduce any steady state error to zero. But motor gear backlash can destabilise things a little.... just depends on motor type.
Well, I did get the timing loop to work (by putting it at the bottom of loop()), but I still got the noisy DMP data. Also, when there wasn't noise, I still got the reversing direction problem. So, still no progress there.
So, you're recommending abandoning the DMP for reading the sensor values directly and doing the math myself. I've been avoiding this for a while. I was wondering why I saw so few Balancing Robots that used on-board position processors. I guess this is why. They don't play well with others. At least not easily enough for beginners to fight through.
Thank you, Southpark for your help. I guess I'm off to a world of trigonometry and Kalman filters. There are a lot of resources on the web for learning this (especially here), so it shouldn't be too bad.
DrKen:
Thank you, Southpark for your help. I guess I'm off to a world of trigonometry and Kalman filters. There are a lot of resources on the web for learning this (especially here), so it shouldn't be too bad.
Most welcome drken. You will find that a kalman filter won't be necessary. If needed.... a complementary filter will do the trick.... very equick and easy to implement. That would only be for combining gyro angles with accelerometer angles. You can certainly get the bot to fairly balance well with accelerometer angles alone. Calibrated measurements are needed.... it is relatively easy to calibrate the mpu6050 accelerometers.
You can always investigate the dmp noisy data issue too. If the calibrated accelerometer angle and gyro angles look fine..... after calibration and testing.... then the issue will likely be with the dmp processing.
You will get your bot working eventually. It is nice to just enjoy the journey of getting the hardware and software to work together. Having the system work straight away would be too boring! You'll get there soon by the look of things.