Ks0377 Keyestudio Balance Car Shield V3
Revision as of 10:43, 23 July 2019 by Keyestudio (talk | contribs)
Description
You might envy those who can easily drive the gravity balance car on the street. So cool! How to DIY a mini balance car on your own?
You are able to combine this shield with UNO R3 control board to make a balance car based on Arduino development platform.
The shield comes with a XBEE Bluetooth socket, so you can connect the Bluetooth APP to easily control the balance car. With the APP, you can choose both key control and gravity control mode, besides, you can also adjust the balance angle and change PID parameters.
The shield is also compatible with a 12V permanent magnet brush motor with a reduction ratio of 1:30 and a locked torque of 7.5kg.cm.
After metal gear reduction output, it has stronger horsepower than ordinary motors. It has low power consumption, high torque and quick start, etc.
For easy settings, the balance car shield integrates a power switch, XBEE communication switch, built-in active buzzer (D11 control), D13 button controlled device, a serial communication and an I2C communication pin.
Technical Details
- Operating Voltage: DC 7-12V
- Operating Current: 800mA
- Compatible with UNO R3 board
- Complying with ROHS
- Dimensions: 80mm*60mm*28mm
- Weight: 28.6g
Element and Pin Interfaces
Here is an explanation figure of what every element and interface of the board has:
Hookup Guide
It is meaningless to test the shield alone. You should stack the shield onto the control board and connect other external devices to build a balance car. Check the picture below.
Reference Code for Balance Car
/*
Contact information
-------------------
*/
#include <Wire.h>
#include "Kalman.h"
#include "ComPacket.h"
#include "I2C.h"
#include <EEPROM.h>
//#include "music.h"
#include "MyEEprom.h"
#include <MPU6050.h>
#include <PinChangeInt.h> // for RC reciver
//#define USEEEPROM //uncomment to use eeprom for storing PID parameters
#define RCWork true
#define mpuAddress 0x68
//Rc receiver //2 channels
#define UP_DOWN_IN_PIN 16
#define LEFT_RIGHT_IN_PIN 17
#define AUX_IN_PIN 14 //is it use RC ,high level :no
//bool RCWork = false;
volatile uint8_t bUpdateFlagsRC = 0;
#define UP_DOWN_FLAG 0b10
#define LEFT_RIGHT_FLAG 0b100
uint32_t UpDownStart;
uint32_t LeftRightStart;
volatile uint16_t UpDownEnd = 0;
volatile uint16_t LeftRightEnd = 0;
int UpDownIn;
int LeftRightIn;
Vector rg, zzl;
Vector_accel calibrate_Accel;
MPU6050 mpu;
//Speaker Mode
#define SPK_OFF 0x00
#define SPK_ON 0x01
#define SPK_ALARM 0x02
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
ComPacket SerialPacket;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
#define BUZZER 6
#define LED 13
char val;
//motor define
#define PWM_L 11 //M1
#define PWM_R 9 //M2
#define DIR_L1 10
#define DIR_L2 12
#define DIR_R1 8
#define DIR_R2 7
//encoder define
#define SPD_INT_R 3 //interrupt
#define SPD_PUL_R 4
#define SPD_INT_L 2 //interrupt
#define SPD_PUL_L 5
int Speed_L, Speed_R;
int Position_AVG;
int Position_AVG_Filter;
int Position_Add;
int pwm, pwm_l, pwm_r;
double Angle_Car;
double Gyro_Car;
double KA_P, KA_D;
double KP_P, KP_I;
double K_Base;
struct EEpromData SavingData;
struct EEpromData ReadingData;
int Speed_Need, Turn_Need;
int Speed_Diff, Speed_Diff_ALL;
uint32_t LEDTimer;
bool blinkState = false;
uint32_t calTimer;
uint32_t delayTimer;
uint32_t lampTimer;
byte speakMode;
//Music play
int tonelength;
int tonePin = 13;
int tonecnt = 0;
uint32_t BuzzerTimer;
void setup() {
//mpu.begin(MPU6050_SCALE_250DPS, MPU6050_RANGE_2G);
Serial.begin(9600);
Init();
//mySerial.begin(9600);
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ��250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ��2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(200); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
// atan2 outputs the value of -�� to �� (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG - calibrate_Accel.pitch;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
/*
if( analogRead(AUX_IN_PIN) > 1000)
{
RCWork = true;
}
else
{
RCWork = false;
}
*/
// using the PinChangeInt library, attach the interrupts
// used to read the channels
Serial.print("RCWork: "); Serial.println(RCWork);
if (RCWork == true)
{
PCintPort::attachInterrupt(UP_DOWN_IN_PIN, calcUpDown, CHANGE);
PCintPort::attachInterrupt(LEFT_RIGHT_IN_PIN, calcLeftRight, CHANGE);
}
PCintPort::attachInterrupt(SPD_INT_L, Encoder_L, FALLING);
PCintPort::attachInterrupt(SPD_INT_R, Encoder_R, FALLING);
timer = micros();
LEDTimer = millis();
lampTimer = millis();
// tonelength = sizeof(tune)/sizeof(tune[0]);
zzl = mpu.calibrateGyro_zzl();
Serial.print("calibrateGyro_zzl = ");
Serial.print(zzl.YAxis);
calibrate_Accel = mpu.calibrate_accel_pitch_and_roll();
Serial.print(" calibrate_Accel = ");
Serial.print(calibrate_Accel.pitch);
digitalWrite(BUZZER, HIGH);
delay(1000);
digitalWrite(BUZZER, LOW);
delay(1000);
while (true)
{
val = Serial.read();
if (val == 0x53)
{
digitalWrite(BUZZER, HIGH);
delay(500);
digitalWrite(BUZZER, LOW);
delay(500);
digitalWrite(BUZZER, HIGH);
delay(500);
digitalWrite(BUZZER, LOW);
delay(500);
digitalWrite(BUZZER, HIGH);
delay(500);
digitalWrite(BUZZER, LOW);
delay(500);
Serial.println("keyes");
break;
}
}
}
void loop() {
double DataAvg[3];
double AngleAvg = 0;
DataAvg[0] = 0; DataAvg[1] = 0; DataAvg[2] = 0;
while (1)
{ //Serial.println("here");
if (UpdateAttitude())
{
DataAvg[2] = DataAvg[1];
DataAvg[1] = DataAvg[0];
DataAvg[0] = Angle_Car;
AngleAvg = (DataAvg[0] + DataAvg[1] + DataAvg[2]) / 3;
if (AngleAvg < 40 || AngleAvg > -40) {
PWM_Calculate();
Car_Control();
}
}
//UserComunication();
SerialEvent();
ProcessRC();
MusicPlay();
}
}
void SerialEvent()
{
char a;
if (Serial.available())
{
a = Serial.read();
//myserial.print(a);
//myserial.print("");
switch (a)
{
case 0x55:
//myserial.println(a);
Speed_Need = 0; Turn_Need = -10;
//Serial.print(Speed_Need); Serial.println(Turn_Need);
break;
case 0x44:
//myserial.println(a);
Speed_Need = 0; Turn_Need = 10;
//Serial.print(Speed_Need); Serial.println(Turn_Need);
break;
case 0x4c:
//myserial.println(a);
Speed_Need = 10; Turn_Need = 0;
//Serial.print(Speed_Need); Serial.println(Turn_Need);
break;
case 0x52:
//myserial.println(a);
Speed_Need = -10; Turn_Need=0;
//Serial.print(Speed_Need); Serial.println(Turn_Need);
break;
case 0x53:
//myserial.println(a);
Speed_Need = 0; Turn_Need = 0;
//Serial.print(Speed_Need); Serial.println(Turn_Need);
break;
default:
break;
}
}
}
bool StopFlag = true;
void PWM_Calculate()
{
float ftmp = 0;
//Serial.print(" Speed_L = ");
//Serial.print(Speed_R);
//Serial.print(" Speed_R = ");
//Serial.print(Speed_L);
//Serial.print(" ftmp = ");
//Serial.print(ftmp);
//Serial.print(" S_L+S_R= ");
//Serial.print(Speed_L + Speed_R);
ftmp = (Speed_L + Speed_R) * 0.5;
//Serial.print(" ftmp = ");
//Serial.print(ftmp);
if (ftmp > 0)
Position_AVG = ftmp + 0.5;
else
Position_AVG = ftmp - 0.5;
Speed_Diff = Speed_L - Speed_R;
Speed_Diff_ALL += Speed_Diff;
//Serial.print(" Speed_Diff_- = ");
//Serial.print(Speed_Diff);
//Serial.print(" Position_AVG = ");
//Serial.print(Position_AVG);
//Serial.print(" Speed_Diff_ALL = ");
//Serial.print(Speed_Diff_ALL);
// Position_AVG_Filter *= 0.5; //speed filter
// Position_AVG_Filter += Position_AVG * 0.5;
Position_AVG_Filter = Position_AVG;
Position_Add += Position_AVG_Filter; //position
//Serial.print(Speed_Need); Serial.print("\t"); Serial.println(Turn_Need);
Position_Add += Speed_Need; //
Position_Add = constrain(Position_Add, -800, 800);
//Serial.print(" Position_Add = ");
//Serial.println(Position_Add);
// Serial.print(Position_AVG_Filter); Serial.print("\t"); Serial.println(Position_Add);
//Serial.print((Angle_Car-5 + K_Base)* KA_P); Serial.print("\t");
pwm = (Angle_Car - 5 + K_Base)* KA_P //P
+ Gyro_Car * KA_D //D
+ Position_Add * KP_I //I
+ Position_AVG_Filter * KP_P; //P
// Serial.println(pwm);
// if(Speed_Need ==0)
// StopFlag = true;
if ((Speed_Need != 0) && (Turn_Need == 0))
{
if (StopFlag == true)
{
Speed_Diff_ALL = 0;
StopFlag = false;
}
pwm_r = int(pwm + Speed_Diff_ALL);
pwm_l = int(pwm - Speed_Diff_ALL);
}
else
{
StopFlag = true;
pwm_r = pwm + Turn_Need; //
pwm_l = pwm - Turn_Need;
}
Speed_L = 0;
Speed_R = 0;
}
void Car_Control()
{
if (pwm_l<0)
{
digitalWrite(DIR_L1, HIGH);
digitalWrite(DIR_L2, LOW);
pwm_l = -pwm_l; //cchange to positive
}
else
{
digitalWrite(DIR_L1, LOW);
digitalWrite(DIR_L2, HIGH);
}
if (pwm_r<0)
{
digitalWrite(DIR_R1, LOW);
digitalWrite(DIR_R2, HIGH);
pwm_r = -pwm_r;
}
else
{
digitalWrite(DIR_R1, HIGH);
digitalWrite(DIR_R2, LOW);
}
if (Angle_Car > 45 || Angle_Car < -45)
{
pwm_l = 0;
pwm_r = 0;
}
// pwm_l = pwm_l; //adjust Motor different
// pwm_r = pwm_r;
// Serial.print(pwm_l); Serial.print("\t"); Serial.println(pwm_r);
analogWrite(PWM_L, pwm_l>180 ? 180 : pwm_l);
analogWrite(PWM_R, pwm_r>180 ? 180 : pwm_r);
/*
Serial.println("pwm:"); Serial.print(pwm);
Serial.print("\t");
Serial.print("pwm_L:"); Serial.print(pwm_l);
Serial.print("pwm_R:"); Serial.print(pwm_r);
*/
}
void UserComunication()
{
MySerialEvent();
if (SerialPacket.m_PackageOK == true)
{
SerialPacket.m_PackageOK = false;
switch (SerialPacket.m_Buffer[4])
{
case 0x01:break;
case 0x02: UpdatePID(); break;
case 0x03: CarDirection(); break;
case 0x04: SendPID(); break;
case 0x05:
SavingData.KA_P = KA_P;
SavingData.KA_D = KA_D;
SavingData.KP_P = KP_P;
SavingData.KP_I = KP_I;
SavingData.K_Base = K_Base;
WritePIDintoEEPROM(&SavingData);
break;
case 0x06: break;
case 0x07:break;
default:break;
}
}
}
void UpdatePID()
{
unsigned int Upper, Lower;
double NewPara;
Upper = SerialPacket.m_Buffer[2];
Lower = SerialPacket.m_Buffer[1];
NewPara = (float)(Upper << 8 | Lower) / 100.0;
switch (SerialPacket.m_Buffer[3])
{
case 0x01:KA_P = NewPara; Serial.print("Get KA_P: \n"); Serial.println(KA_P); break;
case 0x02:KA_D = NewPara; Serial.print("Get KA_D: \n"); Serial.println(KA_D); break;
case 0x03:KP_P = NewPara; Serial.print("Get KP_P: \n"); Serial.println(KP_P); break;
case 0x04:KP_I = NewPara; Serial.print("Get KP_I: \n"); Serial.println(KP_I); break;
case 0x05:K_Base = NewPara; Serial.print("Get K_Base: \n"); Serial.println(K_Base); break;
default:break;
}
}
void CarDirection()
{
unsigned char Speed = SerialPacket.m_Buffer[1];
switch (SerialPacket.m_Buffer[3])
{
case 0x00: Speed_Need = 0; Turn_Need = 0; break;
case 0x01: Speed_Need = -Speed; break;
case 0x02: Speed_Need = Speed; break;
case 0x03: Turn_Need = Speed; break;
case 0x04: Turn_Need = -Speed; break;
default:break;
}
}
void SendPID()
{
static unsigned char cnt = 0;
unsigned char data[3];
switch (cnt)
{
case 0:
data[0] = 0x01;
data[1] = int(KA_P * 100) / 256;
data[2] = int(KA_P * 100) % 256;
AssemblyAndSend(0x04, data);
cnt++; break;
case 1:
data[0] = 0x02;
data[1] = int(KA_D * 100) / 256;
data[2] = int(KA_D * 100) % 256;
AssemblyAndSend(0x04, data);
cnt++; break;
case 2:
data[0] = 0x03;
data[1] = int(KP_P * 100) / 256;
data[2] = int(KP_P * 100) % 256;
AssemblyAndSend(0x04, data);
cnt++; break;
case 3:
data[0] = 0x04;
data[1] = int(KP_I * 100) / 256;
data[2] = int(KP_I * 100) % 256;
AssemblyAndSend(0x04, data);
cnt++; break;
case 4:
data[0] = 0x05;
data[1] = int(K_Base * 100) / 256;
data[2] = int(K_Base * 100) % 256;
AssemblyAndSend(0x04, data);
cnt = 0; break;
default:break;
}
}
void AssemblyAndSend(char type, unsigned char *data)
{
unsigned char sendbuff[6];
sendbuff[0] = 0xAA;
sendbuff[1] = type;
sendbuff[2] = data[0];
sendbuff[3] = data[1];
sendbuff[4] = data[2];
sendbuff[5] = data[0] ^ data[1] ^ data[2];
for (int i = 0; i < 6; i++)
{
Serial.write(sendbuff[i]);
}
}
void Init()
{
pinMode(BUZZER, OUTPUT);
pinMode(SPD_PUL_L, INPUT);//
pinMode(SPD_PUL_R, INPUT);//
pinMode(PWM_L, OUTPUT);//
pinMode(PWM_R, OUTPUT);//
pinMode(DIR_L1, OUTPUT);//
pinMode(DIR_L2, OUTPUT);
pinMode(DIR_R1, OUTPUT);//
pinMode(DIR_R2, OUTPUT);
pinMode(AUX_IN_PIN, INPUT);
pinMode(UP_DOWN_IN_PIN, INPUT);//
pinMode(LEFT_RIGHT_IN_PIN, INPUT);//
//init variables
Speed_L = 0;
Speed_R = 0;
Position_AVG = 0;
Position_AVG_Filter = 0;
Position_Add = 0;
pwm = 0; pwm_l = 0; pwm_r = 0;
Speed_Diff = 0; Speed_Diff_ALL = 0;
//KA_P = 30.0;
//KA_D = 1.0;
//KP_P = 0; //255
//KP_I = 0.0;
//K_Base = 1.5;
//KA_P = 30.0;
//KA_D = 1.3;
//KP_P = 11; //255
//KP_I = 0.01;
//K_Base = 1.7;
//KA_P = 30.0;
//KA_D = 1.0;
//KP_P = 20; //255 good
//KP_I = 0.01;
//K_Base = 1.5;
KA_P = 30.0;
KA_D = 1.0;
KP_P = 20;
KP_I = 0.10;
K_Base = 6.7;
//KA_P = 30.0;
//KA_D = 1.0;
//KP_P = 10;
//KP_I = 0.10;
//K_Base = 2.5;
//KA_P = 30.0;
//KA_D = 1.0;
//KP_P = 10;
//KP_I = 0.20;
//K_Base = 2.5;
ReadingData.KA_P = KA_P;
ReadingData.KA_D = KA_D;
ReadingData.KP_P = KP_P;
ReadingData.KP_I = KP_I;
ReadingData.K_Base = K_Base;
Speed_Need = 0;
Turn_Need = 0;
ReadFromEEprom(&ReadingData);
KA_P = ReadingData.KA_P;
KA_D = ReadingData.KA_D;
KP_P = ReadingData.KP_P;
KP_I = ReadingData.KP_I;
K_Base = ReadingData.K_Base;
Serial.print("PID Data is"); Serial.print("\t");
Serial.print(KA_P); Serial.print("\t");
Serial.print(KA_D); Serial.print("\t");
Serial.print(KP_P); Serial.print("\t");
Serial.print(KP_I); Serial.print("\t");
Serial.print(K_Base); Serial.println("\t");
}
void Encoder_L() //car up is positive car down is negative
{
if (digitalRead(SPD_PUL_L))
Speed_L += 1;
else
Speed_L -= 1;
// Serial.print("SPEED_L: ");
// Serial.println(Speed_L);
}
void Encoder_R() //car up is positive car down is negative
{
if (!digitalRead(SPD_PUL_R))
Speed_R += 1;
else
Speed_R -= 1;
// Serial.print("SPEED_R: ");
// Serial.println(Speed_R);
}
void MySerialEvent()
{
uchar c = '0';
uchar tmp = 0;
if (Serial.available()) {
c = (uchar)Serial.read();
//Serial.println("here");
for (int i = 5; i > 0; i--)
{
SerialPacket.m_Buffer[i] = SerialPacket.m_Buffer[i - 1];
}
SerialPacket.m_Buffer[0] = c;
if (SerialPacket.m_Buffer[5] == 0xAA)
{
tmp = SerialPacket.m_Buffer[1] ^ SerialPacket.m_Buffer[2] ^ SerialPacket.m_Buffer[3];
if (tmp == SerialPacket.m_Buffer[0])
{
SerialPacket.m_PackageOK = true;
}
}
}
}
int UpdateAttitude()
{
if ((micros() - timer) >= 10000)
{ //10ms
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
//Serial.print(" gyroY1 = ");
//Serial.print(gyroY);
//Serial.print(" gyroY_cculate = ");
//Serial.print(gyroY - zzl.YAxis);
/*****************************/
// Wire.beginTransmission(mpuAddress);
//#if ARDUINO >= 100
// Wire.write(MPU6050_REG_GYRO_XOUT_H);
//#else
// Wire.send(MPU6050_REG_GYRO_XOUT_H);
//#endif
// Wire.endTransmission();
//
// Wire.beginTransmission(mpuAddress);
// Wire.requestFrom(mpuAddress, 6);
//
// while (Wire.available() < 6);
//
//#if ARDUINO >= 100
// uint8_t xha = Wire.read();
// uint8_t xla = Wire.read();
// uint8_t yha = Wire.read();
// uint8_t yla = Wire.read();
// uint8_t zha = Wire.read();
// uint8_t zla = Wire.read();
//#else
// uint8_t xha = Wire.receive();
// uint8_t xla = Wire.receive();
// uint8_t yha = Wire.receive();
// uint8_t yla = Wire.receive();
// uint8_t zha = Wire.receive();
// uint8_t zla = Wire.receive();
//#endif
//
// rg.XAxis = xha << 8 | xla;
// rg.YAxis = yha << 8 | yla;
// rg.ZAxis = zha << 8 | zla;
//
// Serial.print(" rg.YAxis = ");
// Serial.print(rg.YAxis);
/*****************************/
/******************************/
/* Vector zzl = mpu.calibrateGyro_zzl();
Serial.print(" calibrateGyro_zzl = ");
Serial.print(zzl.YAxis);*/
/******************************/
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// atan2 outputs the value of -�� to �� (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG - calibrate_Accel.pitch;
//Serial.print(" pitch ="); Serial.print(pitch); Serial.println(" ");
//calibrate_Accel = mpu.calibrate_accel_pitch_and_roll();
//Serial.print(" calibrate_Accel = ");
//Serial.print(calibrate_Accel.pitch);
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = (gyroY - zzl.YAxis) / 131.0; // Convert to deg/s
//Serial.print(" gyroYrate = "); Serial.print(gyroYrate); Serial.print(" ");
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
}
else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
}
else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
// gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
// gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
// compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
//compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
#if 0
Serial.print(roll); Serial.print("\t");
// Serial.print(gyroXangle); Serial.print("\t");
// Serial.print(compAngleX); Serial.print("\t");
Serial.print(kalAngleX); Serial.print("\t");
Serial.print("\t");
Serial.print(pitch); Serial.print("\t");
// Serial.print(gyroYangle); Serial.print("\t");
// Serial.print(compAngleY); Serial.print("\t");
Serial.print(kalAngleY); Serial.println("\t");
#endif
#if 0 // Set to 1 to print the temperature
Serial.print("\t");
double temperature = (double)tempRaw / 340.0 + 36.53;
Serial.print(temperature); Serial.print("\t");
#endif
//Serial.print("\r\n");
Angle_Car = kalAngleY; //negative backward positive forward
Gyro_Car = gyroYrate;
//Serial.print(Angle_Car);Serial.print("\t");Serial.println(Gyro_Car);
return 1;
}
return 0;
}
void LEDBlink()
{
if ((millis() - LEDTimer) > 500)
{
LEDTimer = millis();
blinkState = !blinkState;
// digitalWrite(LED, blinkState);
// Serial.print("Angle_Car\t");
// Serial.println(Angle_Car);//Serial.print("\t");Serial.println(Gyro_Car);
}
}
bool isInMucis = false;
bool isInBuzzer = false;
void MusicPlay()
{
/*
if((Speed_Need !=0 ) || (Turn_Need !=0 )) //when move let's play music
{
if((millis() - BuzzerTimer) >= (380*duration[tonecnt]))
{
noTone(tonePin);
tonecnt++;
if(tonecnt>=tonelength)
tonecnt = 0;
BuzzerTimer = millis();
tone(tonePin,tune[tonecnt]);
isInMucis = true;
}
}
else if(isInMucis == true)
{
isInMucis = false;
noTone(tonePin);
}
*/
if (Angle_Car > 45 || Angle_Car < -45) //if car fall down ,let's alarm
{
if ((millis() - LEDTimer) > 200)
{
LEDTimer = millis();
blinkState = !blinkState;
digitalWrite(LED, blinkState);
isInBuzzer = true;
}
}
else if (isInBuzzer == true)
{
digitalWrite(LED, 0);
isInBuzzer = false;
}
}
//rc receiver interrupt routine
//------------------------------------------------------
void calcUpDown()
{
// Serial.println("in up down here");
if (digitalRead(UP_DOWN_IN_PIN) == HIGH)
{
UpDownStart = micros();
}
else
{
UpDownEnd = (uint16_t)(micros() - UpDownStart);
bUpdateFlagsRC |= UP_DOWN_FLAG;
}
}
void calcLeftRight()
{
// Serial.println("in Left Right here");
if (digitalRead(LEFT_RIGHT_IN_PIN) == HIGH)
{
LeftRightStart = micros();
}
else
{
LeftRightEnd = (uint16_t)(micros() - LeftRightStart);
bUpdateFlagsRC |= LEFT_RIGHT_FLAG;
}
}
#define RC_ERROR 100
#define RC_CAR_SPEED 160
int RCTurnSpeed = 80;
void ProcessRC()
{
if (bUpdateFlagsRC)
{
noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables
if (bUpdateFlagsRC & UP_DOWN_FLAG)
{
UpDownIn = UpDownEnd;
if (abs(UpDownIn - 1500) > RC_ERROR)
{
if (UpDownIn > 1500) // car up
Speed_Need = RC_CAR_SPEED;
else // car down
Speed_Need = -RC_CAR_SPEED;
//Serial.println("here up down");
}
else
{
Speed_Need = 0;
}
}
if (bUpdateFlagsRC & LEFT_RIGHT_FLAG)
{
LeftRightIn = LeftRightEnd;
if (abs(LeftRightIn - 1500) > RC_ERROR)
{
// RCTurnSpeed = 100;//map(ThrottleIn,1000,2000,0,200);
if (LeftRightIn > 1500)
Turn_Need = -RCTurnSpeed; // right
else // left
Turn_Need = RCTurnSpeed;
//Serial.println("here Left Right");
}
else
{
Turn_Need = 0;
}
}
bUpdateFlagsRC = 0;
interrupts();
}
}
Test Result
Upload the test code, you should be able to control the motion of balance car through Android APP.
Resources
- Download the PDF:
https://drive.google.com/open?id=1yCr-fe_3qUMmAI7Pers0m3b_04Lk3I7F
- Download the APP:
https://drive.google.com/open?id=15EISgFSMZV0TxHTHasyLcROiNKYx0TVM
- Download the Test Code:
https://drive.google.com/open?id=1EGO2Q2KsIaWZxa3LJYxIh4VsvLL3vuut
- Download the libraries:
https://drive.google.com/open?id=1sMPt6HdQJSpit_0IegvEn6xlbk754dj9