chrisyang — 2017-10-23 18:53:41 UTC

chrisyang — 2017-10-23 18:55:05 UTC

chrisyang — 2017-10-23 18:58:22 UTC

/*
电路飞翔 - clybot C6寻线自平衡机器人套件
circuitfly - clybot C6 line follower self-balancing robot kit

3.8-带记忆功能的自平衡机器人
3.8-Self-balancing robot with EEPROM

功能说明:
Functionality:
在3.7实验的基础上，使用EEPROM对平衡点进行保存。这样，当clybot C6
断电上再通电，平衡点可以保存着，不需要每次都设置。
Based on the 3.7, EEPROM will be used to store the balancing point.
When the clybot C6 powers off and on again, no need to set the
balancing point again.

电路说明：
circuit:
直流电机、液晶屏等外设的连接如下：
The connections between the peripherals and the Arduino are:
LCD Arduino
| |
CS -- 10
DC -- 8
RST -- 12
MOSI(SDA) -- 11
SCLK(SCK) -- 13

LED -- 6
Speaker -- A3
LDR_L -- A0
LDR_R -- A1
Button_L -- 1
Button_R -- 0
BT TXD -- A3
BT RXD -- 1

该程序由电路飞翔设计
by circuitfly

更多资源请访问：www.clybot.com
More info on: www.clybot.com
*/

#include //声明包含核心图形库 //Core graphics library
#include //声明包含特有硬件库 //Hardware-specific library
#include //声明包含字体 //Font
#include //声明包含I2C通信库 //I2C library
#include //声明包含Kalman库 //Kalman library
#include //声明包含EEPROM库 //EEPROM library

#define TFT_CS 10 //液晶屏管脚连接定义 //LCD pin collection
#define TFT_RST 12
#define TFT_DC 8

//周期性循环设置 //Set the run cycle
#define runEvery(t) for (static long _lasttime;\
(uint16_t)((uint16_t)millis() - _lasttime) >= (t); \
_lasttime += (t))

int SPEAKER = A3; //SPEAKER变量指向管脚A3控制扬声器 //Assign the Pin A3 to variable SPEAKER to control the speaker
int MOT_L_EA = 5; //左侧电机使能端由管脚5控制 //The Pin 5 controls the enable of the left motor
int MOT_L_1 = 7; //左侧电机两个端子由管脚7和9控制 //The Pin 7 & 9 control the terminals of the left motor
int MOT_L_2 = 9;
int MOT_R_EA = 3; //右侧电机使能端由管脚3控制 //The Pin 3 controls the enable of the right motor
int MOT_R_1 = 4; //右侧电机两个端子由管脚4和2控制 //The Pin 4 & 2 control the terminals of the right motor
int MOT_R_2 = 2;
int LED = 6; //LED变量指向管脚6控制发光二极管 //Assign the Pin 6 to LED to control the LED
int Button_L = 0; //Button_L变量指向管脚0接收左按钮开关信号 //Assign the Pin 0 to Button_L to receive the left pushbutton
int Button_R = 1; //Button_R变量指向管脚1接收右按钮开关信号 //Assign the Pin 1 to Button_R to receive the right pushbutton

const int MPU_addr = 0x68; //MPU6050传感器寻址 //I2C address of the MPU6050

//与按钮开关有关的变量 //Set up pushbutton variables
int Button_L_lastState = LOW;
int Button_R_lastState = LOW;
int Button_L_state;
int Button_R_state;

//与加速度计有关的变量 //Set up accelerometer variables
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int16_t accValX, accValY, accValZ;
float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
double accRoll;

//与陀螺仪有关的变量 //Set up gyroscope variables
int16_t gyroX, gyroY, gyroZ;
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroBias_oldX, gyroBias_oldY, gyroBias_oldZ;
float gyroRoll = -180;
int x;
int y;
int z;
double Setpoint;

//设置计时器变量 //Set up a timer Variable
uint32_t timer;

//设置前后倾斜、左右倾斜角度变量 //Set up pitch and roll value variables
double InputRoll;

//设置初始的前后倾斜、左右倾斜角度变量 //Set up initial pitch and roll value variables
double RollInitial;

//新建液晶屏实例tft //Create an LCD instant tft
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_RST);

//新建kalman实例 //Set up kalman instances
Kalman kalmanRoll;

//与液晶屏显示相关变量 //Set up LCD display variables
extern unsigned int Self_balancing[1920];
extern unsigned int Save[361];
uint16_t clybot_r, clybot_b;

/*
显示字符函数 The function for displaying character string
*/
void drawtext(const char *text, uint16_t color, const GFXfont * font, int cursor_x, int cursor_y) {
tft.setFont(font); //设置字体 //Set the font
tft.setCursor(cursor_x, cursor_y); //设置光标位置 //Set the cursor
tft.setTextColor(color); //设置颜色 //Set the colour
tft.setTextWrap(true); //设置自动换行 //Set allowing text wrap
tft.print(text); //打字 //Print the string
}

/*
设备初始化函数 Device initialisation
*/
void initiateDevice() {
Wire.begin(); //启动I2C //Start the I2C
Wire.beginTransmission(MPU_addr); //开始传输 //Begin transmission
Wire.write(0x6B); //写电源管理寄存器 //PWR_MGMT_1 register
Wire.write(0); //唤醒MPU6050 //set to zero (wakes up the MPU6050)
Wire.endTransmission(true); //结束传输 //End transmission

//设置输出管脚 //Set output pins
pinMode(MOT_L_EA, OUTPUT);
pinMode(MOT_L_1, OUTPUT);
pinMode(MOT_L_2, OUTPUT);
pinMode(MOT_R_EA, OUTPUT);
pinMode(MOT_R_1, OUTPUT);
pinMode(MOT_R_2, OUTPUT);
pinMode(LED, OUTPUT);
}

/*
电机控制函数 Motor control function
*/
void MotorControl(double out) {
if (out > 0) {
//正转 //Move forward
digitalWrite(MOT_R_1, HIGH);
digitalWrite(MOT_R_2, LOW);
digitalWrite(MOT_L_1, LOW);
digitalWrite(MOT_L_2, HIGH);
} else {
//反转 //Move backward
digitalWrite(MOT_R_1, LOW);
digitalWrite(MOT_R_2, HIGH);
digitalWrite(MOT_L_1, HIGH);
digitalWrite(MOT_L_2, LOW);
}
byte vel = abs(out);
if (vel < 0) vel = 0;
if (vel > 255) vel = 255;
analogWrite(MOT_R_EA, vel); //PWM控制：变量vel的数值越大，电机转速越快 //PWM control: higher vel, faster motors
analogWrite(MOT_L_EA, vel);

analogWrite(LED, (255 - vel)); //控制发光二极管亮度 //Control LED brightness
//if (vel < 80) tone(SPEAKER, vel * 30); //控制扬声器音调 //Control the speaker tone
//else noTone(SPEAKER);
}

/*
获得传感器数值函数 Update sensor data
*/
void getGyroValues() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); //写加速度输出寄存器ACCEL_XOUT_H //Starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true); //从MPU6050中获得14个寄存器 //Request a total of 14 registers
AcX = Wire.read() << 8 | Wire.read(); //0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY = Wire.read() << 8 | Wire.read(); //0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ = Wire.read() << 8 | Wire.read(); //0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp = Wire.read() << 8 | Wire.read(); //0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
x = Wire.read() << 8 | Wire.read(); //0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
y = Wire.read() << 8 | Wire.read(); //0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
z = Wire.read() << 8 | Wire.read(); //0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

/*
* 显示倾斜角函数 Function for displaying the roll angle
*/
void rollDisplay(double roll) {
tft.fillRect(55, 85, 48, 19, clybot_b);
tft.drawCircle(99, 89, 2, ST7735_WHITE);
String Inroll = String(roll);
char InrollPrintout[7];
Inroll.toCharArray(InrollPrintout, 7);
drawtext(InrollPrintout, ST7735_WHITE, &FreeSansBold9pt7b, 55, 100);
}

/*
计算倾斜角函数 Calculate the roll angle
*/
void getRoll() {
getGyroValues(); //获得更新的传感器数据 //Get updated sensor data
accRoll = (atan2(AcY, -AcZ) + PI) * RAD_TO_DEG; //获得倾斜角 //Get the roll angle
if (accRoll <= 360 & accRoll >= 180) accRoll = accRoll - 360;
gyroRateX = ((int)x - gyroBiasX) * .0105;
gyroRateY = -((int)y - gyroBiasY) * .0105;
gyroRateZ = ((int)z - gyroBiasZ) * .0105;
gyroRoll += gyroRateX * ((double)(micros() - timer) / 1000000);
RollInitial = kalmanRoll.getAngle(accRoll, gyroRoll, (double)(micros() - timer) / 1000000);
rollDisplay(RollInitial);
timer = micros();
Setpoint = 0;
}

/*
计算在C6移动前的MPU6050的数值 Calculate bias for the Gyro i.e. the values it gives when it's not moving
*/
void calculateBias() {
for (int i = 1; i < 100; i++) { //循环100次后取平均 //Repeat 100 times and average
getGyroValues(); //获得传感器MPU6050的原始数据 //Obtain original data from the MPU6050
gyroBiasX += x; //把陀螺仪的x,y,z数值进行累加 //Accumulate the x, y, z values in the gyro
gyroBiasY += y;
gyroBiasZ += z;
delay(1); //延时1毫秒 //Delay for 1 ms
}
gyroBiasX = gyroBiasX / 100; //把数值进行平均 //Average all the values
gyroBiasY = gyroBiasY / 100;
gyroBiasZ = gyroBiasZ / 100;

getGyroValues(); //获得传感器MPU6050的原始数据 //Obtain original data from the MPU6050
accRoll = (atan2(AcY, -AcZ) + PI) * RAD_TO_DEG;

if (accRoll <= 360 & accRoll >= 180) accRoll = accRoll - 360; //对超范围值的处理 //Process values outside the range

//设置kalman起始角度// Set starting angle for Kalman
kalmanRoll.setAngle(accRoll);
kalmanRoll.setQangle(0.01);
kalmanRoll.setQbias(0.0003);
kalmanRoll.setRmeasure(0.01);
gyroRoll = accRoll;
timer = micros(); //计时 //Timing
delay(1000);
getRoll(); //获得倾斜角 //Get the pitch and roll angles
}

int outMax = 255;
int outMin = -255;
float lastInput = 0;
double ITerm = 0;
double kp = 100;
double ki = 2;
double kd = 0;

/*
计算PID输出函数 Compute PID output
*/
double Compute(double input) {
double error = Setpoint - input;
ITerm += (ki * error);
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
double dInput = (input - lastInput);
double output = kp * error + ITerm + kd * dInput;
if (output > outMax) output = outMax;
else if (output < outMin) output = outMin;
lastInput = input;
return output;
}

/*
液晶屏显示欢迎界面函数 LCD welcome interface
*/
void initiateLCD() {
tft.initR(INITR_144GREENTAB); //启动1.44寸液晶屏 //Initiate the 1.44' LCD
tft.fillScreen(ST7735_WHITE); //白色填充屏幕 //Fill the LCD with white
clybot_r = tft.Color565(224, 58, 2); //获得红、蓝两种颜色值 //Get the red and blue colours
clybot_b = tft.Color565(1, 137, 149);
tft.drawBitmap (16, 64, 32, 60, Self_balancing); //显示clybot C6图标 //The clybot C6 icon
tft.drawBitmap (105, 85, 19, 19, Save);
for (int i = 0; i < 20; i++) { //屏幕上方两个方条动画 //Animations of the two bars
tft.fillRect(0, 0, 6 * i, 25, clybot_b);
tft.fillRect(128 - 6 * i, 30, 6 * i, 25, clybot_r);
}
drawtext("Balancing", clybot_r, &FreeSansBold9pt7b, 12, 17); //显示文字 //Display characters
drawtext("clybot C6", ST7735_WHITE, &FreeSansBold9pt7b, 32, 47);
drawtext("B Point", clybot_r, &FreeSansBold9pt7b, 55, 75);
}

/*
最先执行的设置函数 //Setup function runs first
*/
void setup() {
initiateDevice(); //设备初始化函数 //Device initialisation
initiateLCD(); //液晶屏欢迎界面显示函数 //LCD welcome UI

int EEPROM_status;
EEPROM_status = EEPROM.read(0); //把EEPROM地址0状态读出 //Read EEPROM address = 0
if (EEPROM_status == 88) { //如果状态为88，说明已经保存有平衡点角度 //If data = 88, the balancing point has been stored previously
double EEPROM_RollInitial;
EEPROM.get(1, EEPROM_RollInitial); //把地址1上保存的平衡点角度读出 //Get the initial roll angle from the address = 1
RollInitial = EEPROM_RollInitial; //把平衡点角度保存到RollInitial中 //Pass the saved roll angle to RollInitial
rollDisplay(EEPROM_RollInitial); //显示平衡角度 //Display roll angle
}
else calculateBias(); //如果之前没有保存平衡点角度，则计算初始偏移量函数 //Calculate initial bias if not previously saved
}

int i = 0;
double aaa = 0;

void loop() {
runEvery(10) {
//检测按钮开关 //Read buttons
Button_L_state = digitalRead(Button_L);
Button_R_state = digitalRead(Button_R);
if (Button_L_state == LOW) RollInitial += 0.02;
if (Button_R_state == LOW) RollInitial -= 0.02;
if ((Button_L_state == LOW) || (Button_R_state == LOW)) { //如果按钮按下，更新EEPROM //If button pressed, update the EEPROM
EEPROM.write(0, 88); //向地址0写入平衡点保存标记88 //Write the flag of 88 as the roll angle saved
EEPROM.put(1, RollInitial); //向地址1保存平衡点角度 //Save roll angle to address = 1
double EEPROM_RollInitial;
EEPROM.get(1, EEPROM_RollInitial); //从地址1读出数据传递给液晶屏显示 //Read the roll angle from address = 1 to display on the LCD
rollDisplay(EEPROM_RollInitial);
}

getGyroValues(); //获得更新的传感器数据 //Get updated sensor data
accRoll = (atan2(AcY, -AcZ) + PI) * RAD_TO_DEG; //获得倾斜角 //Get the roll angle
if (accRoll <= 360 & accRoll >= 180) accRoll = accRoll - 360;
gyroRateX = -((int)x - gyroBiasX) * .0105;
gyroRateY = -((int)y - gyroBiasY) * .0105;
gyroRateZ = ((int)z - gyroBiasZ) * .0105;

double leeeeel = gyroRateX * ((double)(micros() - timer) / 1000000);
gyroRoll += gyroRateX * ((double)(micros() - timer) / 1000000);
InputRoll = kalmanRoll.getAngle(accRoll, gyroRoll, (double)(micros() - timer) / 1000000);
timer = micros();
byte a = map(abs(Compute(InputRoll - RollInitial)), 0, 255, 0, 124);
aaa = 0.98 * (aaa + leeeeel) + 0.02 * (accRoll);
MotorControl(Compute(aaa - RollInitial));
}
}

clybot C6 by Chris Yang