Hacking Freestyle 4x4 R/C Car. Make Your Own Standing Robot

After we saw this Botka-Standing Robot on Sparkfun website.
Just for fun we bought two form Discovery Store try to do the same for our CopyArts project.
We still working on it...[Just need put Gyroscope,Accelerometer & MCU together]

This is current version in-store we have

This is Orginal GyroMax ATV , Botka use

2 main components are changed (1)Remote TX/RX chip set (2) 4 Half-Hbridge -> Dual 4 transistor H-bridge

These are Discovery Freestyle R/C 4x4 key components

• Remote IC SCTX2B/SCRX2FS 27MHz [Digital Output R/C Chip set]
  
• Battery NiCd 6V [ X mAH, no idea how big is it..:p]
  
• Motor Dual 260 Motors [I am looking for datasheet]
  
• Dual 4 transistor H-bridge [B1658 PNP+ B2583 NPN]

R/C Functions

Left,Right H-Brdge is controlled via SCRX2FS PIN10,11;6,7

R/C Key	SCRX2FS P10	SCRX2FS P11	Left Motor Direction
Left Middle	Low	Low	Stop
Left Up	Low	Hi	Forward
Left Down	Hi	Low	Backward

R/C Key	SCRX2FS P7	SCRX2FS P6	Right Motor Direction
Right Middle	Low	Low	Stop
Right Up	Low	Hi	Forward
Right Down	Hi	Low	Backward

RX side, main components on PCB [use view image to see full size image ]

• Step 1

Remove 4 resistors R13,R14,R25,R26

• Step2

Add 4 Input,4 Output line to your MCU

• Step3

Add Power Supply to power your circuit. Connect 6V,GND

• Step4

MCU connection
R/C output (O) -----> MCU Digital Input (4 bit)
H-Bridge Control(I) <----- 4 16bit PWM channel
6V -------> to Regulator and 1/3 voltage divider input to MCU A/D channel
GND -GND

• Step5

Add your MCU,Accelerometer,Gyroscope

• Step6

Coding........

• Step 7

Have fun ....

Wiring location for Step 1-3

******************************************************
Reference:
System equitation and source code
These equation & code are form Eric Defelice www.mil.ufl.edu/~defelice/ Project Final Report
******************************************************

State equations for balancing controller is

M = mass of base
m = mass of pendulum
b = friction
l = length from floor to center of mass
I = inertia of robot
u = force applied to robot
x = robot position
Φ = tilt angle

Controller code from Matlab

M = 0.5;
m = 0.2;
b = 0.1;
i = 0.006;
g = 9.8;
l = 0.3;
p = i*(M+m)+M*m*l^2; %denominator
A = [0 1 0 0;
0 -(i+m*l^2)*b/p (m^2*g*l^2)/p 0;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p 0];
B = [0; (i+m*l^2)/p; 0; m*l/p];
C = [1 0 0 0;
0 0 1 0];
D = [0;0];
p = eig(A)
x=6000;
y=200;
Q=[x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
K = lqr(A,B,Q,R)
Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];

T=0:0.01:5;
U=0.2*ones(size(T));
Cn=[1 0 0 0];
Nbar=rscale(A,B,Cn,0,K)
Bcn=[Nbar*B];
P = [-40 -41 -42 -43];
L = place(A',C',P)'
Ace = [A-B*K B*K;
zeros(size(A)) (A-L*C)];
Bce = [ B*Nbar;
zeros(size(B))];
Cce = [Cc zeros(size(Cc))];
Dce = [0;0];
T = 0:0.01:5;
U = 0.2*ones(size(T));
[Y,X] = lsim(Ace,Bce,Cce,Dce,U,T);
plot(T,Y)


Kalman Filter C-Code

void tilt_init(tilt *self, float dt, float R_angle, float Q_gyro, float Q_angle)
// Initialize the kalman state.
{
// Initialize the two states, the angle and the gyro bias.
self->bias = 0.0;
self->rate = 0.0;
self->angle = 0.0;

// Initialize the delta in seconds between gyro samples.
self->dt = dt;

// Initialize the measurement noise covariance matrix values.
// In this case, R is a 1x1 matrix tha represents expected
// jitter from the accelerometer.
self->R_angle = R_angle;

// Initialize the process noise covariance matrix values.
// In this case, Q indicates how much we trust the acceleromter
// relative to the gyros.
self->Q_gyro = Q_gyro;
self->Q_angle = Q_angle;

// Initialize covariance of estimate state. This is updated
// at every time step to determine how well the sensors are
// tracking the actual state.
self->P_00 = 1.0;
self->P_01 = 0.0;
self->P_10 = 0.0;
self->P_11 = 1.0;
}

void tilt_state_update(tilt *self, float gyro_rate)
// Our state vector is:
// X = [ angle, gyro_bias ]
//
// It runs the state estimation forward via the state functions:
//
// Xdot = [ angle_dot, gyro_bias_dot ]
//
// angle_dot = gyro - gyro_bias
// gyro_bias_dot = 0
//
// And updates the covariance matrix via the function:
//
// Pdot = A*P + P*A' + Q
//
// A is the Jacobian of Xdot with respect to the states:
//
// A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]
// [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]
//
// = [ 0 -1 ]
// [ 0 0 ]
{
// Static so these are kept off the stack.
static float gyro_rate_unbiased;
static float Pdot_00;
static float Pdot_01;
static float Pdot_10;
static float Pdot_11;

// Unbias our gyro.
gyro_rate_unbiased = gyro_rate - self->bias;

// Compute the derivative of the covariance matrix
//
// Pdot = A*P + P*A' + Q
Pdot_00 = self->Q_angle - self->P_01 - self->P_10;
Pdot_01 = -self->P_11;
Pdot_10 = -self->P_11;
Pdot_11 = self->Q_gyro;

// Store our unbiased gyro estimate.
self->rate = gyro_rate_unbiased;

// Update the angle estimate.
//
// angle += angle_dot * dt
// += (gyro - gyro_bias) * dt
// += q * dt
//
self->angle += gyro_rate_unbiased * self->dt;

// Update the covariance matrix.
self->P_00 += Pdot_00 * self->dt;
self->P_01 += Pdot_01 * self->dt;
self->P_10 += Pdot_10 * self->dt;
self->P_11 += Pdot_11 * self->dt;
}

void tilt_kalman_update(tilt *self, float angle_measured)
// The C matrix is a 1x2 (measurements x states) matrix that
// is the Jacobian matrix of the measurement value with respect
// to the states. In this case, C is:
//
// C = [ d(angle_measured)/d(angle) d(angle_measured)/d(gyro_bias) ]
// = [ 1 0 ]
{
// Static so these are kept off the stack.
static float angle_error;
static float C_0;
static float PCt_0;
static float PCt_1;
static float E;
static float K_0;
static float K_1;
static float t_0;
static float t_1;

// Compute the error in the estimate.
angle_error = angle_measured - self->angle;

// C_0 shows how the state measurement directly relates to
// the state estimate.
//
C_0 = 1.0
// PCt<2,1> = P<2,2> * C'<2,1>, which we use twice. This makes
// it worthwhile to precompute and store the two values.
PCt_0 = C_0 * self->P_00; /* + C_1 * P_01 = 0 */
PCt_1 = C_0 * self->P_10; /* + C_1 * P_11 = 0 */

// Compute the error estimate. From the Kalman filter paper:
//
// E = C P C' + R
//
// Dimensionally,
//
// E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>
//
E = self->R_angle + C_0 * PCt_0;

// Compute the Kalman filter gains. From the Kalman paper:
//
// K = P C' inv(E)
//
// Dimensionally:
//
// K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>
//
// Luckilly, E is <1,1>, so the inverse of E is just 1/E.
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

//
// Update covariance matrix. Again, from the Kalman filter paper:
//
// P = P - K C P
//
// Dimensionally:
//
// P<2,2> -= K<2,1> C<1,2> P<2,2>
//
// We first compute t<1,2> = C P. Note that:
//
// t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0]
//
// But, since C_1 is zero, we have:
//
// t[0,0] = C[0,0] * P[0,0] = PCt[0,0]
//
// This saves us a floating point multiply.
t_0 = PCt_0;
t_1 = C_0 * self->P_01;
self->P_00 -= K_0 * t_0;
self->P_01 -= K_0 * t_1;
self->P_10 -= K_1 * t_0;
self->P_11 -= K_1 * t_1;

//
// Update our state estimate. Again, from the Kalman paper:
//
// X += K * err
//
// And, dimensionally,
//
// X<2> = X<2> + K<2,1> * err<1,1>
//
// err is a measurement of the difference in the measured state
// and the estimate state. In our case, it is just the difference
// between the two accelerometer measured angle and our estimated
// angle.
self->bias += K_1 * angle_error;
self->angle += K_0 * angle_error;
}