User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 9:45 am

The chunk of code below is directly out of the original script and it is wrong

Code: Select all

for (i = 0; i < dataSize;) {
        // determine correct chunk size according to bank position and data size
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

        // make sure we don't go past the data size
        if (i + chunkSize > dataSize) chunkSize = dataSize - i;

        // make sure this chunk doesn't go past the bank boundary (256 bytes)
        if (chunkSize > 256 - address) chunkSize = 256 - address;
        
        if (useProgMem) {
            // write the chunk of data as specified
            for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
        } else {
            // write the chunk of data as specified
            progBuffer = (uint8_t *)data + i;
        }

        I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);

        // verify data if needed
        if (verify && verifyBuffer) {
            setMemoryBank(bank);
            setMemoryStartAddress(address);
            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                /*Serial.print("Block write verification error, bank ");
                Serial.print(bank, DEC);
                Serial.print(", address ");
                Serial.print(address, DEC);
                Serial.print("!\nExpected:");
                for (j = 0; j < chunkSize; j++) {
                    Serial.print(" 0x");
                    if (progBuffer[j] < 16) Serial.print("0");
                    Serial.print(progBuffer[j], HEX);
                }
                Serial.print("\nReceived:");
                for (uint8_t j = 0; j < chunkSize; j++) {
                    Serial.print(" 0x");
                    if (verifyBuffer[i + j] < 16) Serial.print("0");
                    Serial.print(verifyBuffer[i + j], HEX);
                }
                Serial.print("\n");*/
                free(verifyBuffer);
                if (useProgMem) free(progBuffer);
                return false; // uh oh.
            }
        }

        // increase byte index by [chunkSize]
        i += chunkSize;

        // uint8_t automatically wraps to 0 at 256
        address += chunkSize;

Specifically this line if (chunkSize > 256 - address) chunkSize = 256 - address; and how it affects this line address += chunkSize;. There is no doubt that this never worked and therefore the only possible way this was working was with the useProgMem flag set to true (if even that). There is no way around the fact that as it currently stands, once address gets to 256 ~ 256 - address equals 0 and chunkSize is greater than 0 so it gets set to 0. Then later when address is incremented by chunkSize it is incremented by 0. That state is maintained forever, so as this original method stands, it will write the first 256 bytes and then just loop forever doing nothing, since address will be 256 forever. This never worked. It's gonna work now though. Really the logic here is stupid. It should always keep working up by 16 (MPU6050_DMP_MEMORY_CHUNK_SIZE) unless there isn't 16 left. I'm going to rewrite the entire method. Comparing chunksize to address is entirely unnecessary. And actually this line (not shown above) if (address == 0) bank++; would indicate that the chunkSize increment should actually be address = (address + chunkSize) % 256, as that check if address is 0 is directly after it's increment.

See, it's not just "port it whether you understand it or not". It's "understand every iota of it because some of it isn't even right". There's no denying that the above script is broken. I haven't bothered to trace down the ProgMem part since Pico doesn't have that, but that might not work either. If it does, though ~ that's the only reason this worked, at all. Honestly though, just judging by this ProgMem line for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); I don't see how it possible could have worked, because chunkSize is still 0. This can only mean that literally nobody is using DMP for this script or figured it was so far over their head that they were doing something wrong. This method is used to write the firmware to the device, and as we can clearly see, it will only get 256 bytes into that.


on a side note: below is my DMP math rewrite so, that's a thing that happened. He hard-coded the 2 into his script and then left a comment in one method that it actually represent the current sensitivity. I made that one method dynamic to the actual sensitivity (although there are number of ways you can perceive that 2 and I'm not sure I'm using the right "2"). Then the next functions used a bunch of 2 and I'm not sure if it's just 2 or if those also represent sensitivity. I made it where I can quickly change all "2"s (v), in case I have to do that. I still need to add the LinearAccel methods, and one of them will be very simple, for the other I need to bust out the math book and get the formula for vector rotation, cause I haven't messed with that in a while and I don't remember it. It's something like (do * stuff + here / probably) :D

Code: Select all

    
    def dmpGetGyro32(self, packet:bytearray) -> namedtuple:
        return _POINTDATA(int.from_bytes(packet[16:20], _E), int.from_bytes(packet[20:24], _E), int.from_bytes(packet[24:28], _E))
    
    def dmpGetGyro16(self, packet:bytearray) -> namedtuple:
        return _POINTDATA(int.from_bytes(packet[16:18], _E), int.from_bytes(packet[20:24], _E), int.from_bytes(packet[24:26], _E))
    
    def dmpGetAccel32(self, packet:bytearray) -> namedtuple:
        return _POINTDATA(int.from_bytes(packet[28:32], _E), int.from_bytes(packet[32:36], _E), int.from_bytes(packet[36:40], _E))
    
    def dmpGetAccel16(self, packet:bytearray) -> namedtuple:
        return _POINTDATA(int.from_bytes(packet[28:30], _E), int.from_bytes(packet[32:34], _E), int.from_bytes(packet[36:38], _E))
    
    def dmpGetQuaternion32(self, packet:bytearray) -> namedtuple:
        return _VECTOR(int.from_bytes(packet[0:4], _E), int.from_bytes(packet[4:8], _E), int.from_bytes(packet[8:12], _E), int.from_bytes(packet[12:16], _E))

    def dmpGetQuaternion16(self, packet:bytearray) -> namedtuple:
        return _VECTOR(int.from_bytes(packet[0:2], _E), int.from_bytes(packet[4:6], _E), int.from_bytes(packet[8:10], _E), int.from_bytes(packet[12:14], _E))

    def dmpGetGravity(self, packet:bytearray) -> namedtuple:
        # +1g corresponds to +8192, sensitivity is 2g.
        d = int(self.__accsense * 8192)
        q = self.dmpGetQuaternion16(packet)
        x = (q.x * q.z - q.w * q.y) / d
        y = (q.w * q.x + q.y * q.z) / d
        z = (q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) / (2 * d)
        return _POINTDATA(x, y, z)

    def dmpGetEuler(self, q:namedtuple) -> namedtuple:
        v = 2 #self.__accsense
        x = math.atan2(v * q.x * q.y - v * q.w * q.z, v * q.w * q.w + v * q.x * q.x - 1)   # psi
        y = -math.asin(v * q.x * q.z + v * q.w * q.y)                              # theta
        z = math.atan2(v * q.y * q.z - v * q.w * q.x, v * q.w * q.w + v * q.z * q.z - 1)   # phi
        return _POINTDATA(x, y, z)
        
    def dmpGetYawPitchRoll(self, q:namedtuple, g:namedtuple) -> namedtuple:
        v = 2 #self.__accsense
        x = math.atan2(v * q.x * q.y - v * q.w * q.z, v * q.w * q.w + v * q.x * q.x - 1)     # yaw: (about Z axis)
        y = math.atan2(g.x , math.sqrt(g.y * g.y + g.z * g.z))                               # pitch: (nose up/down, about Y axis)
        z = math.atan2(g.y , g.z)                                                            # roll: (tilt left/right, about X axis)
        if g.z < 0:
            y = (math.pi - y) if(y > 0) else (-math.pi - y)
        return _POINTDATA(x, y, z)
        
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 10:38 am

It works great when you tell it to do the right thing. I'm about to go over every last one of his "actually doing something" methods with great scrutiny. My confidence in his skill level is very low after this blatant error in his code. I mean, this was just a loop that needed to write some bytes in chunks of 16, and it read like a stackoverflow question. That's just not acceptable.I really don't have any faith in his PID method now. A.K.A the method that is giving whacked out numbers for offsets. I also need to find this firmware. I don't trust the changes he made to that either. If somebody feels like being my assistant for a minute and tracking that down for me so I can keep programming, that would be awesome. If not, I'll get to it.

However what would maybe be even more helpful is finding out if we even need the firmware or if he just changed a bunch of stuff and is overwriting the firmware that is already there. If he is just overwriting the existing firmware (with probably some arduino specifics), then maybe we could completely skip this step. He claims it's volatile and needs to be done every time. I'm not sure if that means it gets erased on power off or reverted on power off.

Image

Code: Select all

   def writeMemoryBlock(self, data, dataSize:int, bank:int=0, address:int=0, verify:bool=True) -> bool:
        self.setMemoryBank(bank)
        self.setMemoryStartAddress(address)
        
        vbuff = None if not verify else bytearray()
        
        i, chunkSize = 0, _DMP_MEMORY_CHUNK_SIZE
        while i < dataSize:
            if i+_DMP_MEMORY_CHUNK_SIZE > dataSize:
                chunkSize = dataSize - i
                
            self.writeBytes(_RA_MEM_R_W, data[i:i+chunkSize])
    
            if verify and (not vbuff is None):
                self.setMemoryBank(bank)
                self.setMemoryStartAddress(address)
                vbuff = self.readBytes(_RA_MEM_R_W, chunkSize)
                if not (len(vbuff) == chunkSize):
                    print('problems v:{}, cs:{}'.format(len(vbuff), chunkSize))
                    return False #	 uh oh.
                
            i      += chunkSize
            address = (address + chunkSize) % 256
            if i < dataSize:
                bank += int(not address)
                self.setMemoryBank(bank)
                self.setMemoryStartAddress(address)
        
        return True
        
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 12:01 pm

However what would maybe be even more helpful is finding out if we even need the firmware or if he just changed a bunch of stuff and is overwriting the firmware that is already there. If he is just overwriting the existing firmware (with probably some arduino specifics), then maybe we could completely skip this step. He claims it's volatile and needs to be done every time. I'm not sure if that means it gets erased on power off or reverted on power off.


Nevermind, I answered my own question with code. I powered cycled the device and read from the firmware banks, then I wrote his firmware and read them again, then I powered cycled it and read them, 4 or 5 more times. I then wrote a script that saves the firmware bytes to textfiles where everything is hexed, separated by banks and formatted to 16 columns. By observation some kind of firmware is written every time the device gets power, but it's not the same thing every time. By opening up all my printouts, scrolling them to the exact same position and then switching tabs I could see that there were key spots that were always different while major chunks always remained the same.

My guess is that the device attempts to do some form of calibration, or at least recording of some initial values, on it's own, and since this is the most unstable device that was ever invented it's never the same values. However, there is definitely some kind of firmware written on every power on. Too much of it was identical and thee specific spots that changed were too consistent to believe it's all just junk. This begs another question, though: If my guess is correct that the device is recording some initial values on every power up, how reliable could a static firmware be? It is overwriting a bunch of init data that is likely key to allowing the device to function properly. That's IF I'm correct, and if I am, then his firmware is a waste of time and space.

The picture I am starting to get from this J Rowberg script is that he was in the "port it whether you understand it or not" crowd, and because of that some of his script is broken, and some of his decisions were bad. He thought that rewriting the firmware would save some setup, but I believe (due to his his "just do it" way of writing it) that he's actually crippling the device. His firmware overwrite has nothing in place that leaves certain registers alone, and due to the device changing those registers on every power up this tells me his firmware is wrong. It's a "moment in time" that he is treating like "the law", and I really don't believe it's that simple.
Last edited by OneMadGypsy on Sun Jun 06, 2021 12:11 pm, edited 1 time in total.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 12:09 pm

Could it be related to the POST ??

7.12 Self-Test
Please refer to the MPU-6000/MPU-6050 Register Map and Register Descriptions document for more details on self test.
Self-test allows for the testing of the mechanical and electrical portions of the sensors. The self-test for each measurement axis can be activated by means of the gyroscope and accelerometer self-test registers (registers 13 to 16).
When self-test is activated, the electronics cause the sensors to be actuated and produce an output signal. The output signal is used to observe the self-test response.
The self-test response is defined as follows:
Self-test response = Sensor output with self-test enabled – Sensor output without self-test enabled
The self-test response for each accelerometer axis is defined in the accelerometer specification table (Section 6.2), while that for each gyroscope axis is defined in the gyroscope specification table (Section 6.1).
When the value of the self-test response is within the min/max limits of the product specification, the part has passed self test. When the self-test response exceeds the min/max values, the part is deemed to have failed self-test. Code for operating self test code is included within the MotionApps software provided by InvenSense.

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 12:14 pm

I read that before, and it would be nothing for me to do a self test because all the functions to do those tests already exist. This IS the MotionApps software provided by InvenSense. or at least as much of it as J Rowberg was capable of porting... now ported to micropython. Maybe I should have just ported the InvenSense version. Using my transpiler I could have ported most of the entire thing. I would have had to write it for C, though. And I don't feel like my C skills are very good. My C++ skills are only very slightly better, and really only if we are talking about modern C++.

I'm glad you brought this up. I'm looking at the below code that I took for granted to be correct, and it isn't. Well, it isn't consistent. AccelZSelfTestFactoryTrim is doing it's own thing and it's the wrong thing. This is a double whammy. This is a spot where the MSB is being sent last AND a spot where he changes the game on how he gets values. Pbbbbt. Mr. J Rowberg needs to get his act together. I need to go check the values and see if TEST_A is right after TEST_Z. If it is then his way almost makes sense, but he still should have kept it consistent with AccelX and Y

Code: Select all

   def AccelXSelfTestFactoryTrim(self) -> int:
        x = self.readByte(_RA_SELF_TEST_X)
        a = self.readByte(_RA_SELF_TEST_A)	
        return (x>>3) | ((a>>4) & 0x03)

    @property
    def AccelYSelfTestFactoryTrim(self) -> int:
        y = self.readByte(_RA_SELF_TEST_Y)
        a = self.readByte(_RA_SELF_TEST_A)	
        return (y>>3) | ((a>>2) & 0x03)    

    @property
    def AccelZSelfTestFactoryTrim(self) -> int:
        buff = self.readBytes(_RA_SELF_TEST_Z, 2)	
        return (buff[1]>>3) | (buff[0] & 0x03)

    @property
    def GyroXSelfTestFactoryTrim(self) -> int:
        return (self.readByte(_RA_SELF_TEST_X) & 0x1F)

    @property
    def GyroYSelfTestFactoryTrim(self) -> int:
        return (self.readByte(_RA_SELF_TEST_Y) & 0x1F)

    @property
    def GyroZSelfTestFactoryTrim(self) -> int:
        return (self.readByte(_RA_SELF_TEST_Z) & 0x1F)

    @property
    def AccelXSelfTest(self) -> bool:
        return self.readBit(_RA_ACCEL_CONFIG, _ACONFIG_XA_ST_BIT)

    def setAccelXSelfTest(self, enabled:bool) -> None:
        self.writeBit(_RA_ACCEL_CONFIG, _ACONFIG_XA_ST_BIT, enabled)

    @property
    def AccelYSelfTest(self) -> bool:
        return self.readBit(_RA_ACCEL_CONFIG, _ACONFIG_YA_ST_BIT)

    def setAccelYSelfTest(self, enabled:bool) -> None:
        self.writeBit(_RA_ACCEL_CONFIG, _ACONFIG_YA_ST_BIT, enabled)

    @property
    def AccelZSelfTest(self) -> bool:
        return self.readBit(_RA_ACCEL_CONFIG, _ACONFIG_ZA_ST_BIT)

    def setAccelZSelfTest(self, enabled:bool) -> None:
        self.writeBit(_RA_ACCEL_CONFIG, _ACONFIG_ZA_ST_BIT, enabled)



edit: A is after Z, so I understand his micro-optimization, but I don't agree with it.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 2:05 pm

I have the device almost properly calibrated. Everything is very close to zero, except for some reason AccelZ calibrates to 1 a very very solid 1, but 1 none-the-less. Imma go eat something and come back to wrap this up. I believe I will have this script finished today.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 2:35 pm

That sounds very good :D :D

horuable
Posts: 121
Joined: Sat Mar 06, 2021 12:35 am

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 3:03 pm

OneMadGypsy wrote: I have the device almost properly calibrated. Everything is very close to zero, except for some reason AccelZ calibrates to 1 a very very solid 1, but 1 none-the-less. Imma go eat something and come back to wrap this up. I believe I will have this script finished today.
From the datasheet:
When the device is placed on a flat surface, it will measure
0g on the X- and Y-axes and +1g on the Z-axis.
So I guess everything is just as it should be.

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 3:16 pm

Thats correct, its showing the earth's gravity :)

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 3:25 pm

Thanks, horuable! You know I would have made this thing zero. You just saved me so much unnecessary work. Good lookin' out.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 4:19 pm

I kept power cycling the Pico and running calibration. The numbers were always identical. I commented out the calibration and hardcoded the offsets. The image below shows my results. I think this is as good as it gets. These results are with the device sitting perfectly flat and still. Since the device is on a breadboard I threw a 100nF decoupling capacitor across Vcc and Vss right up against the MPU-6050. I don't think it made any difference cause these are basically the same results I was already getting. I also checked the datasheet, it doesn't seem like any of the unused pins are supposed to be tied to ground. If so, I can't find the spot where it says to do it.

Image


Edit: making sure the device is absolutely level with the earth when doing calibration is key. I just got this thing as flat as I possibly could using a bubble level, and the results from that calibration gave me even better results than the ones above. it seemed to flutter between 0.08 and -0.04.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 4:47 pm

Great stuff, I don't think there is any such thing as perfection with these devices, they are pretty remarkable little things after all. Even my phone wobbles a bit in level and compass, you can only go so far i think.

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 5:56 pm

I can't seem to get the fifo buffer to give me any data. The count on the buffer is always 0. I've quadruple checked that every related thing is doing the exact same thing as the original. I'm not really sure what to do. I didn't come this far just to have an entire feature crippled by one buffer. I think it's time to go to the datasheet and see what it tells me about the fifo.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 7:01 pm

Man. yet another spot in his code that is wrong. For one BUFFER_LENGTH isn't defined anywhere in the cpp or the h, and no other files are included, so it just isn't defined.For two ... What? Man, read those last 2 while chunks and realize how much they don't make any sense, at all. That whole chunk is the equivalent of very elaborately saying "never-mind. Let's slowly and insanely forget about the whole thing with bad math." I get what he's trying to do here ... read off a bunch of excess so there isn't an overflow problem. Got it. Unfortunately, this isn't going to work. This doesn't work, and never did. Why is everybody basing their stuff on this guy's code. A notable amount of it is wrong ... like just about everything that deals with DMP. This dude was supposed to be "The Champ". This garbage shouldn't even be in the ring.

Code: Select all

if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
                 resetFIFO(); // Fixes any overflow corruption
                 fifoC = 0;
                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (11000))); // Get Next New Packet
            } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
                 uint8_t Trash[BUFFER_LENGTH];
                 while ((fifoC = getFIFOCount()) > length) {  // Test each time just in case the MPU is writing to the FIFO Buffer
                     fifoC = fifoC - length; // Save the last packet
                     uint16_t  RemoveBytes;
                     while (fifoC) { // fifo count will reach zero so this is safe
                         RemoveBytes = min((int)fifoC, BUFFER_LENGTH); // Buffer Length is different than the packet length this will efficiently clear the buffer
                         getFIFOBytes(Trash, (uint8_t)RemoveBytes);
                         fifoC -= RemoveBytes;
                     }
                 }

On the side: I've got it reading out of the FIFO, but it's not the right values. I'm just happy it's finally doing something. I'll figure out the glitch soon enough.
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Sun Jun 06, 2021 8:04 pm

Why is everybody basing their stuff on this guy's code.
My guess would be that nobody has has either had the knowledge or the time or the willpower to do it? I suppose the code sort of works ok, maybe the bugs don't show up in the uses most of us put it to, I mean, mobile phones etc are not going to be running an Arduino with a duff library on it are they :D :D

hippy
Posts: 9958
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

Re: Beginner - help with self-balancing robot build

Mon Jun 07, 2021 1:32 pm

OneMadGypsy wrote:
Sun Jun 06, 2021 7:01 pm
Man. yet another spot in his code that is wrong. For one BUFFER_LENGTH isn't defined anywhere in the cpp or the h, and no other files are included,
So how does it get through compilation ?

I am not sure what source code is actually being used. I can't find a link to it when I look through the thread but that may just be 'link blindness'. A reminder link would be appreciated.
OneMadGypsy wrote:
Sun Jun 06, 2021 7:01 pm
Why is everybody basing their stuff on this guy's code. A notable amount of it is wrong ... like just about everything that deals with DMP. This dude was supposed to be "The Champ". This garbage shouldn't even be in the ring.
Looking at some of the code, especially that which 'does nothing', I get the impression this is or was a work in progress, that the author was making additions to investigate things which never came to fruition, haven't yet been completed.

All of that, warts and all, is what seems to have been published. I don't think it's fair to call it garbage if that is the case, and particularly if it was the only thing available at some point in time, or best there was at that time. Did the author ever claim to be The Champ, or was that praise heaped on him by others for what he had so far achieved ?

I imagine others have used his code because they found it good enough for themselves, saw no need to rewrite his library or didn't have the skills and knowledge to do so. "There's no need to fix what doesn't seem broken" likely plays a huge part as well

I sometimes look inside libraries because I am curious. Most people don't, just assume it will work, do as desired, and judge on whether it appears to or not. I guess most people deemed it worked well enough that they used it and recommended it to others. It could well be a case of saying a cake tastes great without realising the nonsense involved in creating it. Its not how it's done but what gets delivered, with 'good enough' being a win.

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Mon Jun 07, 2021 2:51 pm

I like it when things work - managed to get a Bluetooth device running today, cheapest HC-05 i could find and it works on my Android phone :D I never knew they were just serial modems before 8-)

Nothing to control with it yet but its fun to play with and learn I guess

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Tue Jun 08, 2021 11:36 am

Still playing with this, have now managed to get an amalgamation of my interrupt-callback stepper drive and interrupt-callback IMU idea to run together, slightly pleased so far. Still not usable though due to poor IMU data as before, its just all over the place, I really cannot see how this is so easy on an Arduino yet fails on a Pico.

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Wed Jun 09, 2021 3:23 pm

Ok, so using the library from OneMadGypsy (many thanks) and the code below, I can now read both gyro and accelerometer and convert to angles. As seems normal, the gyro drifts and at present i just force it back to zero as the accelerometer passes zero, probably a bad idea but i'll leave it for now.

Code: Select all

from machine import Pin, Timer
import utime
import math
from mpu6050 import MPU6050 

global angleX
angleX = 0
global mixed
mixed = 0
global gyangle
gyangle = 0


# Setup the MPU6050 IMU
sda = 14
scl = 15
bus = 1
gyro_range = 0 # range [250, 500, 1000, 2000]
accel_range = 0 # range [2, 4, 8, 16]
d_rate = 32
d_dlpf = 3 # = DLPF_BW_42
offsets = (-620, 3115, 1370, 70, -15, 119) # From manual calibration run
mpu = MPU6050(bus, sda, scl, ofs=offsets, gyro=gyro_range, accel=accel_range, rate=d_rate, dlpf=d_dlpf)


# self test
# print(MPU6050(1, 14, 15, ofs=offsets).passed_self_test)

# Used for angle calcs to convert radians to degrees
rad2degree = 180 / math.pi



# Fixed timer loop for measuring Gyro angle change
def tick(timer):
    global angleX
    global mixed
    global gyangle

    ax, ay, az, gx, gy, gz = mpu.data                  # Get a single reading from the IMU                           

    angleX = math.atan2(ax, az) * rad2degree           # Calculate X angle from accelerometer data
                    
    if (angleX > -0.01 and angleX < 0.01): gyangle = 0 # Reset the gyro reading to cancel drift

    gyangle = gyangle + gy * 0.02                      # convert gyro deg/sec to degrees..... eg. timer freq = 100Hz so 1/100=0.01 loop time

    # not sure if this helps or not 
    mixed = 0.96 * gyangle + 0.04 * angleX             # Combine readings - 96% gyro, 4% accelerometer


Timer().init(freq=50, mode=Timer.PERIODIC, callback=tick) # repeating timer 


#  Main program loop
if mpu.passed_self_test:
    while True:

        print("Acc:{:.2f}  Gyr:{:.2f}".format(angleX , gyangle))        

        utime.sleep_ms(100)


davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Thu Jun 10, 2021 11:40 am

Connected it all up again today, it did sort of try to balance, for a short while which was pretty encouraging :D

It seems I have many issues:-

The IMU data is still very poor, too much vibration effect, too much drift still. It cannot function if the drift means its off by maybe 5 degrees after only a few seconds. Not sure yet how to tackle this.

The motor speed seems too low, maybe need bigger wheels but certainly need better control as my current idea is not going to work i think. I think to get more speed i need to figure out ramping as they are pretty at the stall limit now for a straight start/stop.

I have seen many builds with Nema17's so i know it can be done but still can't figure out how as they are all Arduino builds as usual. The other option is geared DC motors with encoders but I think that adds a further layer of complexity and not sure if its worth it simply because there are many builds with stepper motors. I can't find out how fast the motors need to turn yet either.

davek0974
Posts: 211
Joined: Mon Jul 22, 2019 1:52 pm

Re: Beginner - help with self-balancing robot build

Fri Jun 11, 2021 1:30 pm

LOL, I spent several hours today figuring out how to get my stepper speeds to ramp up, much to my pleasure I managed it quite nicely in the end with a timer interrupt callback loop and it worked very well.

Of course, after doing that I realised that it was not needed if the motor speeds are connected to the tilt angle - small angle = low speed, speed increases with tilt angle - its pretty much got built-in ramping :D :D

Oh well, its another block of code for the scrapbook i guess :)

Changed the code again and will test later.

hippy
Posts: 9958
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

Re: Beginner - help with self-balancing robot build

Fri Jun 11, 2021 2:48 pm

davek0974 wrote:
Fri Jun 11, 2021 1:30 pm
Of course, after doing that I realised that it was not needed if the motor speeds are connected to the tilt angle - small angle = low speed, speed increases with tilt angle - its pretty much got built-in ramping :D :D
I believe I said that some time ago, but that's not criticism because one often has to undertake the journey to comprehend things oneself, reach the epiphany.

From "small angle = low speed, large angle = higher speed" one can take a further step that "angles" themselves may not actually be needed, raw readings may be just as proportionate; "small number = low speed, larger number = higher speed". That's not to say there won't be any processing involved to determine what the tilt number is, but it could be a lot simpler than it seemed it had to to be.

As said; I have never done it but have pondered how it could be done. If movement and speed is proportional to tilt angle it doesn't seem a PID algorithm is entirely necessary.

But I am prepared to accept - even expect - that such simplistic thinking will give poor results, and refinements such as PID and better tilt readings and greater motor controls are necessary for providing better control.

This is why I have said my approach would not be to port refined code, but to deconstruct it, figure out what the fundamental principles are, then move forward with implementing that.

User avatar
OneMadGypsy
Posts: 265
Joined: Wed Apr 28, 2021 1:57 am
Location: New Orleans, Louisiana
Contact: Website

Re: Beginner - help with self-balancing robot build

Fri Jun 11, 2021 4:28 pm

If movement and speed is proportional to tilt angle it doesn't seem a PID algorithm is entirely necessary.

Right. Until you turn on the device without it and it claims its at a 27 degree tilt when it's at a 1 degree tilt and the data it spits out every poll is more like random number generation. Seriously, to say these devices "boot up" in WTF mode is an understatement. Also, you need an algorithm that works in both directions, cause negative directions exist, and more negative is not "more", but it needs to be. Just throwing abs at the number is not good enough cause the motor needs to spin the other way. Of course you can always do "something", but it's all a lot nicer when you are doing the same something that professional robot builders are doing. Device calibration is a standard. There are also standard methods of doing that calibration.


This is with no calibration
Image

The device is at rest, but those numbers are the equivalent of it "stunt driving". It may not seem like fluctuating between .90 and .88 (accY) is a big deal, until you realize your range under normal G forces is -1 to 1, meaning that's a 1.8 degree difference "at rest". If you don't calibrate, you're gonna have big problems. Having some kind of smoothing filter is necessary too or your robot will never stop adjusting for movements it didn't even make, which throws the number off even more, and then the entire system completely falls apart if it hasn't already.

Here's my device calibrated. The accelerometer values are all that matter as that is where the pitch and roll values come from. There's no fluctuation beyond it flipping between negative and positive. I don't even have a Kalman filter on this, as my code only applies that to angles and those aren't shown here. Of course beyond the second decimal there is some fluctuation. However that 3rd decimal is only necessary if you want accuracy of .09 of a degree. Which maybe you do, and if so it might make sense to put a filter on these values, too.
Image
"Focus is a matter of deciding what things you're not going to do." ~ John Carmack

hippy
Posts: 9958
Joined: Fri Sep 09, 2011 10:34 pm
Location: UK

Re: Beginner - help with self-balancing robot build

Fri Jun 11, 2021 6:22 pm

OneMadGypsy wrote:
Fri Jun 11, 2021 4:28 pm
The accelerometer values are all that matter as that is where the pitch and roll values come from.
That is something which has intrigued me which you may be able to enlighten me on; how roll comes into it.

For a typical solid construction two-wheeled self-balancing robot on a flat surface I can't see how it could roll. From its perspective only forward and backward lean or pitch would seem to exist.

In the days before the MPU6050 the best people had were rotary encoders and pots measuring lean from an actual pendulum which would only give one channel of reading and those seemed to work.

emma1997
Posts: 1525
Joined: Sun Nov 08, 2015 7:00 pm
Location: New England (not that old one)

Re: Beginner - help with self-balancing robot build

Fri Jun 11, 2021 6:37 pm

davek0974 wrote:
Thu Jun 10, 2021 11:40 am
The motor speed seems too low, maybe need bigger wheels
I've been following this thread from start and find it fascinating having built a few myself back in the Trinity College Contest days. I suspect, if anything, smaller wheels would be better. More torque and faster response w/o stalling. Are you ramping up/down the steppers properly?

IMO the language chosen may not be best for this type work. I found Python (or any interpreter) very difficult to do time critical applications and prefer C or ASM. PID easy for either of those last two. Also found hand tuned complementary filters preferable to Kalman.

Of course this was back in the old days before everybody just depended on prefab libraries. We had to write all our own from scratch back then. And beware getting trampled by Mastodons. lol

Return to “MicroPython”