Balancing Car Project Attempt number 99
Over the number of years now I have been dabbling with make a balancing robot work with the P1. I seem to always get hung up on the timing with the Gyro Scope and the Accelerometer.
Anyway the other day I came across this chassis that look nice and was fairly inexpensive.
Now how do I drive this thing. Parallax sells a motor driver MC33926 for $90 dollars. What that's a little steep for a motor controller. I check from the manufacturer's website and it sells for $65 there. Still a little pricey. Then I found this board:
This board is only $7. At that price I took two.
Now to drive this board is a snap. You only need 5 wires, Ground, M1A, M1B, M2A, M2B. That's it.
I went to work a coded up a driver for it in just a couple of minutes.
#include <stdio.h>
#include <stdlib.h>
#include "propeller.h"
#include "smartpins.h"
#include "mdd3a.h"
int _M1A, _M1B, _M2A, _M2B;
void mdd3a_Init(int m1a, int m1b, int m2a, int m2b)
{
int f;
f = 500 | 500 << 16;
_M1A = m1a;
_M1B = m1b;
_M2A = m2a;
_M2B = m2b;
_pinstart(_M1A, P_OE | P_PWM_TRIANGLE, f, 0);
_pinstart(_M1B, P_OE | P_PWM_TRIANGLE, f, 0);
_pinstart(_M2A, P_OE | P_PWM_TRIANGLE, f, 0);
_pinstart(_M2B, P_OE | P_PWM_TRIANGLE, f, 0);
}
void mdd3a_SetPWM(int pwm1, int pwm2)
{
int set1, set2;
if (pwm1 < 1000)
pwm1 = 1000;
if (pwm1 > 2000)
pwm1 = 2000;
if (pwm2 < 1000)
pwm2 = 1000;
if (pwm2 > 2000)
pwm2 = 2000;
set1 = pwm1 - 1500;
set2 = pwm2 - 1500;
if (abs(set1) < 5)
set1 = 0;
if (abs(set2) < 5)
set2 = 0;
if (set1 < 0)
{
_wypin(_M1A, 0);
_wypin(_M1B, -set1);
}
else
{
_wypin(_M1B, 0);
_wypin(_M1A, set1);
}
if (set2 < 0)
{
_wypin(_M2A, 0);
_wypin(_M2B, -set2);
}
else
{
_wypin(_M2B, 0);
_wypin(_M2A, set2);
}
}
void mdd3a_Stop(void)
{
_pinf(_M1A);
_pinf(_M1B);
_pinf(_M2A);
_pinf(_M2B);
}
Smart pins made the job easier. The hard part was figuring out what all those codes mean to configure the smart pin and while I was at it, I made the driver compatible with an RC receiver.
Now for the hard part. I decided to use an ICM20948 chip. This is the re-engineered MPU9250 from Invensense which is now a part of TDK. Same frustrating chip with a new twist.
Anyway, I like a challenge and this chip aims to deliver. After working on it for about a week now I think I have the chip working enough to use in my project. The driver is massive with all the crazy stuff in the chip. It has a DMP processor on it but to use it require a lot more memory and time that I don't need to look at. The documentation is poor, and the code is only available in binary format.
For this project I only need the Gyro scope and the Accelerometer to work.
Here is a picture of the complete robot ready to fall over.
The most expensive piece is the P2. Now I need to build some loop code that can make this beast stand up on its own.
Mike
Comments
Please post links for the chassis and boards?
I got them through digikey but they are made by Seeed Technology.
Two Wheel Car
Maker MDD3A Motor Controller
Seeed Studio Website
All of those items and more are out of stock on the Seeed website.
Mike
Nice! Good luck on this, looking forward to seeing her balance.
Got the code put together and the robot looks like it's had too many to drink.
This is where I'm lost. Is it too much P or not enough I or is it just D.
The code has been tuned to the loop time which is about 1.3ms to read the gyro and accelerometer and calculate the speed percentage which is converted to an RC PWM value.
I got the initial code off the internet and then modified it for my use. It's using a complementary filter and not Kalman. Don't want to make this complicated just yet.
Here is the code that I'm using:
#include <stdio.h> #include <stdlib.h> #include <math.h> #include <propeller.h> #include "icm20948.h" #include "mdd3a.h" #define ICMCLK 36 #define ICMDTA 37 #define DMM1A 20 #define DMM1B 21 #define DMM2A 22 #define DMM2B 23 float compFilter(float gyroX, float accY, float accZ); int i; //Complementary Filter vars, filter weight K const float K = 0.998F; float pitch = 0.0F; //Loop Time Value const float DT = 0.001395F; // 0.001250 const float KP = 2.5; //2.5F const float KI = (30.0F*0.001395f); const float KD = (0.05F/0.001395f); float setPoint = -0.3F; int main(int argc, char** argv) { int ms; short ax, ay, az; short gx, gy, gz; float currentError, previousError; float integral = 0.0F; float derivative = 0.0F; int speedPercent = 0; int pwm; i = ICM20948_Init(ICMCLK, ICMDTA); if (i != 0) { printf("ICM20948 Not Found!\n"); while (1) _waitms(500); } ICM20948_SetGyroOffsets(20, -13, 8); ICM20948_SetAccelOffsets(2450, 3496, -660); /* 4G */ ICM20948_ConfigAccel(0, 1, 1); /* 2000DPS */ ICM20948_ConfigGyro(0, 3, 1); mdd3a_Init(DMM1A, DMM1B, DMM2A, DMM2B); _waitms(2000); printf("Starting...\n"); i = 1; while (1) { ms = _getus(); while ((gz = ICM20948_DataReady()) == 0) _waitus(1); if (gz != 3) _pinl(57); ICM20948_Gyro(&gx, &gy, &gz); ICM20948_Accel(&ax, &ay, &az); pitch = compFilter(gx/16, ay, az); currentError = (setPoint - pitch); integral = (integral + currentError); derivative = (currentError - previousError); speedPercent = ((KP * currentError) + (KI * integral) + (KD * derivative)); if (speedPercent > 50) speedPercent = 50; if (speedPercent < -50) speedPercent = -50; pwm = (speedPercent * 5) + 1500; mdd3a_SetPWM(pwm, pwm); previousError = currentError; ms = _getus() - ms; if (abs(pitch) > 20) { mdd3a_Stop(); _pinh(57); printf("Pulse: %d\n", pwm); while (1) _waitms(500); } } } float compFilter(float gyroX, float accY, float accZ) { float acc = (180.0F / PI) * atan2(accY, accZ); // printf("Gyro: %3.1f, Angle: %3.1f\n", gyroX, acc); // _waitms(5000); return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * (acc))); }Here is a video of the drunken robot:
Balancing Bot
You can see that it is bouncing back and forth.
Mike
Nice!
When I experimented with such a balancing robot, the most astonishing finding was: If you enlarge the distance between the wheels and the balance point, making the robot higher, the stability will improve. Well, inertia will increase and base frequency will go down.
Battery on top!