#include "SoftwareSerial.h"
#include "Servo.h"
/////////////////BLDC Motor/////////////////
#define MAX_THROTTLE 179
#define MIN_THROTTLE 0
#define MAX_PWM 100
#define MIN_PWM 20
#define MOTOR1 5 //motor pin number
#define MOTOR2 9 //motor pin number
#define MOTOR3 10 //motor pin number
#define MOTOR4 3 //motor pin number
int motor1Power, motor3Power; // Motors on the X axis
//=>; changing their speeds will affect the pitch
int motor2Power, motor4Power; // Motors on the Y axis
//=>; changing their speeds will affect the roll
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
////////////////////////////////////////////
/////////////////IMU Sensor/////////////////
#define DATANUM 12 //6
#define ACC_X_OFFSET 0.18
#define ACC_Y_OFFSET 0.01
#define ACC_Z_OFFSET -0.77
//IMU Sensor Serial
SoftwareSerial SensorSerial(2,4);//Rx Tx
//IMU Sensor variable
typedef union {
signed short value;
unsigned char byte[2];
} S2BYTE;
S2BYTE data_word;
unsigned char SBuf[100];
int SCnt=0;
double DegRoll, DegPitch, DegYaw;
double Acc_x, Acc_y, Acc_z;
////////////////////////////////////////////
/////////////////PID Conrtroll/////////////////
#define MINhover 0
#define INIThover 20
#define INITROLLKp 0.38
#define INITROLLKi 0.000287
#define INITROLLKd 33
#define INITPITCHKp 0.29
#define INITPITCHKi 0.000287
#define INITPITCHKd 33
#define INITYAWKp 0.29 //0.38
#define INITYAWKi 0.000287
#define INITYAWKd 33
#define MAXSPEED 3
#define MINSPEED 100
#define INITROLLSETPOINT -1.532
#define INITPITCHSETPOINT -0.516
double roll_setpoint = INITROLLSETPOINT, pitch_setpoint = INITPITCHSETPOINT, yaw_setpoint = 0;
int hover = INIThover;
double roll_Kp = INITROLLKp, roll_Ki = INITROLLKi, roll_Kd = INITROLLKd;
double pitch_Kp = INITPITCHKp, pitch_Ki = INITPITCHKi, pitch_Kd = INITPITCHKd;
double yaw_Kp = INITYAWKp, yaw_Ki = INITYAWKi, yaw_Kd = INITYAWKd;
double roll_p, roll_i, roll_d, roll_val;
double pitch_p, pitch_i, pitch_d, pitch_val;
double yaw_p, yaw_i, yaw_d, yaw_val;
int rollControll, pitchControll, yawControll;
int initial = 0;
////////////////////////////////////////////
/////////////////Serial Monitor/////////////////
int Serial_print_sig=1;
int roll_print = 1, pitch_print =0, yaw_print=0, hover_print=0;
////////////////////////////////////////////
//---------------------------------------------------------------------//
//
// funtions
//
//--------------------------------------------------------------------//
//esc조절 하기
void callibration(void)
{
Serial.println("Calibrate ESC? 'y' or 'n'.");
while (!Serial.available());
char cali = Serial.read();
if(cali == 'y'){
Serial.println("send max throttle");
motor1.attach(MOTOR1);
motor2.attach(MOTOR2);
motor3.attach(MOTOR3);
motor4.attach(MOTOR4);
motor1.write(MAX_THROTTLE);
motor2.write(MAX_THROTTLE);
motor3.write(MAX_THROTTLE);
motor4.write(MAX_THROTTLE);
delay(5000);
Serial.println("send min throttle");
motor1.attach(MOTOR1);
motor2.attach(MOTOR2);
motor3.attach(MOTOR3);
motor4.attach(MOTOR4);
motor1.write(MIN_THROTTLE);
motor2.write(MIN_THROTTLE);
motor3.write(MIN_THROTTLE);
motor4.write(MIN_THROTTLE);
delay(2000);
Serial.println("callibration complete");
}else if(cali == 'n'){
motor1.attach(MOTOR1);
motor2.attach(MOTOR2);
motor3.attach(MOTOR3);
motor4.attach(MOTOR4);
motor1.write(MIN_THROTTLE);
motor2.write(MIN_THROTTLE);
motor3.write(MIN_THROTTLE);
motor4.write(MIN_THROTTLE);
}
Serial.println("If you want Program keep going. Press any key.");
while (!Serial.available());
}
//------------------------------------------------------------------------//
//센서값을 읽어서 roll,pitch,yaw 값을 알아낸다.
int recieve_data(void)
{
static int step_=0, check_all_recieve=0;
unsigned char data1byte;
int n;
data1byte = SensorSerial.read();
switch(step_)
{
case 0:
if(data1byte==0x55) {step_=1, check_all_recieve=0;}
break;
case 1:
if(data1byte==0x55) { step_=2; SCnt=0;}
else step_=0;
break;
case 2:
SBuf[SCnt++] = (unsigned char)data1byte;
if(SCnt==DATANUM+2) // roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte)
{
step_=0;
SCnt=0;
check_all_recieve = 1;
}
break;
default:
break;
}
return check_all_recieve;
}
static int trans_data_to_degree(unsigned char *sdata) // sdata : roll(2)+pitch(2)+yaw(2)+chk(2)
{
int n;
unsigned short chk=0;
///////////// checksum /////////////////
chk=0x55+0x55;
for(n=0;n<DATANUM;n++) chk+=sdata[n];
if(chk != get_data_word(&sdata[DATANUM]))
{
Serial.println("checksum error!");
return 0; //
}
////////////////////////////////////////
DegRoll = get_data_word(&sdata[0]) / 100.;
DegPitch = get_data_word(&sdata[2]) / 100. * -1;
DegYaw = get_data_word(&sdata[4]) / 100.;
Acc_x = get_data_word(&sdata[6]) / 100. + ACC_X_OFFSET;
Acc_y = get_data_word(&sdata[8]) / 100. + ACC_Y_OFFSET;
Acc_z = get_data_word(&sdata[10]) / 100. + ACC_Z_OFFSET;
return 1;
}
static signed short get_data_word(unsigned char *dat)
{
// little endian..
data_word.byte[0]=dat[1];
data_word.byte[1]=dat[0];
return data_word.value;
}
//--------------------------------------------------------------------------------//
//각도 값을 받아서 PID제어값 출력
double roll_pid(double setpoint, double degree)
{
double val_out;
static double roll_p_err_prv, roll_p_err, roll_i_err, roll_d_err;
roll_p_err = setpoint-degree;
roll_i_err += roll_p_err;
roll_d_err = roll_p_err - roll_p_err_prv;
roll_p_err_prv = roll_p_err;
val_out = (roll_Kp*roll_p_err) + (roll_Ki*roll_i_err) + (roll_Kd*roll_d_err);
roll_p = roll_Kp*roll_p_err;
roll_i = roll_Ki*roll_i_err;
roll_d = roll_Kd*roll_d_err;
roll_val = val_out;
return val_out;
}
double pitch_pid(double setpoint, double degree)
{
double val_out;
static double pitch_p_err_prv, pitch_p_err, pitch_i_err, pitch_d_err;
pitch_p_err = setpoint-degree;
pitch_i_err += pitch_p_err;
pitch_d_err = pitch_p_err - pitch_p_err_prv;
pitch_p_err_prv = pitch_p_err;
val_out = (pitch_Kp*pitch_p_err) + (pitch_Ki*pitch_i_err) + (pitch_Kd*pitch_d_err);
pitch_p = pitch_Kp*pitch_p_err;
pitch_i = pitch_Ki*pitch_i_err;
pitch_d = pitch_Kd*pitch_d_err;
pitch_val = val_out;
return val_out;
}
double yaw_pid(double setpoint, double degree)
{
double val_out;
static double yaw_p_err_prv, yaw_p_err, yaw_i_err, yaw_d_err;
yaw_p_err = setpoint-degree;
yaw_i_err += yaw_p_err;
yaw_d_err = yaw_p_err - yaw_p_err_prv;
yaw_p_err_prv = yaw_p_err;
val_out = (yaw_Kp*yaw_p_err) + (yaw_Ki*yaw_i_err) + (yaw_Kd*yaw_d_err);
yaw_p = yaw_Kp*yaw_p_err;
yaw_i = yaw_Ki*yaw_i_err;
yaw_d = yaw_Kd*yaw_d_err;
yaw_val = val_out;
return val_out;
}
//------------------------------------------------------------------------
//제어값을 받어서 모터 구동
void run_motors(int throttle, int rollOffset, int pitchOffset, int yawOffset)
{
motor1Power = throttle + pitchOffset - yawOffset;
motor2Power = throttle + rollOffset + yawOffset;
motor3Power = throttle - pitchOffset - yawOffset;
motor4Power = throttle - rollOffset + yawOffset;
limit_max_min(motor1Power);
limit_max_min(motor2Power);
limit_max_min(motor3Power);
limit_max_min(motor4Power);
motor1.write(motor1Power);motor3.write(motor3Power);
motor2.write(motor2Power);motor4.write(motor4Power);
}
void limit_max_min(int pwm)
{
if(pwm>MAX_PWM)
pwm = MAX_PWM;
else if(pwm<MIN_PWM)
pwm = MIN_PWM;
}
//-------------------------------------------------------------------------------//
void Serial_Monitor(void)
{
if(Serial_print_sig ==1 ){
if(roll_print == 1){
Serial.print("Roll=> ");Serial.print(DegRoll);Serial.print(", ");Serial.print(roll_p); Serial.print(", ");Serial.print(roll_i); Serial.print(", ");Serial.print(roll_d);Serial.print(", ");Serial.print(roll_val);Serial.print(", ");Serial.print(motor2Power);Serial.print(", ");Serial.println(motor4Power);
}else if(pitch_print == 1){
Serial.print("Pitch=> ");Serial.print(DegPitch);Serial.print(", ");Serial.print(pitch_p); Serial.print(", ");Serial.print(pitch_i); Serial.print(", ");Serial.print(pitch_d);Serial.print(", ");Serial.print(pitch_val);Serial.print(", ");Serial.print(motor1Power);Serial.print(", ");Serial.println(motor3Power);
}else if(yaw_print == 1){
Serial.print("Yaw=> ");Serial.print(DegYaw);Serial.print(", ");Serial.print(yaw_p); Serial.print(", ");Serial.print(yaw_i); Serial.print(", ");Serial.print(yaw_d);Serial.print(", ");Serial.println(yaw_val);
}
}
if(Serial_print_sig == 0){
if(hover_print == 1){
Serial.print("hover = ");Serial.println(hover);
hover_print =0;
}else if(roll_print == 1){
Serial.print("roll_Kp = ");Serial.print(roll_Kp);Serial.print(", roll_Ki = ");Serial.print(roll_Ki*10000);Serial.print(", roll_Kd = ");Serial.println(roll_Kd);
roll_print = 0;
}else if(pitch_print == 1){
Serial.print("pitch_Kp = ");Serial.print(pitch_Kp);Serial.print(", pitch_Ki = ");Serial.print(pitch_Ki*10000);Serial.print(", pitch_Kd = ");Serial.println(pitch_Kd);
pitch_print = 0;
}else if(yaw_print == 1){
Serial.print("yaw_Kp = ");Serial.print(yaw_Kp);Serial.print(", yaw_Ki = ");Serial.print(yaw_Ki*10000);Serial.print(", yaw_Kd = ");Serial.println(yaw_Kd);
yaw_print = 0;
}
}
}
//------------------------------------------------------------------------------//
void from_computer(void)
{
char c = Serial.read();
//hover debuging
if(c=='1'){
hover += 1;
Serial.print("hover = ");Serial.println(hover);
}else if(c=='2'){
hover -= 1;
Serial.print("hover = ");Serial.println(hover);
}else if(c=='3'){
hover= INIThover;
Serial.print("hover = ");Serial.println(hover);
}else if(c=='4'){
hover= MINhover;
Serial.print("hover = ");Serial.println(hover);
}else if(c=='5'){
roll_setpoint += 0.1;
Serial.print("roll_setpoint = ");Serial.println(roll_setpoint);
}else if(c=='6'){
roll_setpoint -= 0.1;
Serial.print("roll_setpoint = ");Serial.println(roll_setpoint);
}else if(c=='7'){
roll_setpoint = INITROLLSETPOINT;
Serial.print("roll_setpoint = ");Serial.println(roll_setpoint);
}
//roll debuging
else if(c=='q'){
roll_Kp += 0.01;
Serial.print("roll_Kp = ");Serial.println(roll_Kp);
}else if(c=='w'){
roll_Kp -= 0.01;
Serial.print("roll_Kp = ");Serial.println(roll_Kp);
}else if(c=='e'){
roll_Kp= INITROLLKp;
Serial.print("roll_Kp = ");Serial.println(roll_Kp);
}else if(c=='a'){
roll_Ki += 0.000001;
Serial.print("roll_Ki = ");Serial.println(roll_Ki*10000);
}else if(c=='s'){
roll_Ki -= 0.000001;
Serial.print("roll_Ki = ");Serial.println(roll_Ki*10000);
}else if(c=='d'){
roll_Ki = INITROLLKi;
Serial.print("roll_Ki = ");Serial.println(roll_Ki*10000);
}else if(c=='z'){
roll_Kd += 1;
Serial.print("roll_Kd = ");Serial.println(roll_Kd);
}else if(c=='x'){
roll_Kd -= 1;
Serial.print("roll_Kd = ");Serial.println(roll_Kd);
}else if(c=='c'){
roll_Kd = INITROLLKd;
Serial.print("roll_Kd = ");Serial.println(roll_Kd);
}
//pitch debuging
else if(c=='r'){
pitch_Kp += 0.01;
Serial.print("pitch_Kp = ");Serial.println(pitch_Kp);
}else if(c=='t'){
pitch_Kp -= 0.01;
Serial.print("pitch_Kp = ");Serial.println(pitch_Kp);
}else if(c=='y'){
pitch_Kp= INITPITCHKp;
Serial.print("pitch_Kp = ");Serial.println(pitch_Kp);
}else if(c=='f'){
pitch_Ki += 0.000001;
Serial.print("pitch_Ki = ");Serial.println(pitch_Ki*10000);
}else if(c=='g'){
pitch_Ki -= 0.000001;
Serial.print("pitch_Ki = ");Serial.println(pitch_Ki*10000);
}else if(c=='h'){
pitch_Ki = INITPITCHKi;
Serial.print("pitch_Ki = ");Serial.println(pitch_Ki*10000);
}else if(c=='v'){
pitch_Kd += 1;
Serial.print("pitch_Kd = ");Serial.println(pitch_Kd);
}else if(c=='b'){
pitch_Kd -= 1;
Serial.print("pitch_Kd = ");Serial.println(pitch_Kd);
}else if(c=='n'){
pitch_Kd = INITPITCHKd;
Serial.print("pitch_Kd = ");Serial.println(pitch_Kd);
}
//yaw debuging
else if(c=='u'){
yaw_Kp += 0.01;
Serial.print("yaw_Kp = ");Serial.println(yaw_Kp);
}else if(c=='i'){
yaw_Kp -= 0.01;
Serial.print("yaw_Kp = ");Serial.println(yaw_Kp);
}else if(c=='o'){
yaw_Kp= INITYAWKp;
Serial.print("yaw_Kp = ");Serial.println(yaw_Kp);
}else if(c=='j'){
yaw_Ki += 0.000001;
Serial.print("yaw_Ki = ");Serial.println(yaw_Ki*10000);
}else if(c=='k'){
yaw_Ki -= 0.000001;
Serial.print("yaw_Ki = ");Serial.println(yaw_Ki*10000);
}else if(c=='l'){
yaw_Ki = INITYAWKi;
Serial.print("yaw_Ki = ");Serial.println(yaw_Ki*10000);
}else if(c=='m'){
yaw_Kd += 1;
Serial.print("yaw_Kd = ");Serial.println(yaw_Kd);
}else if(c==','){
yaw_Kd -= 1;
Serial.print("yaw_Kd = ");Serial.println(yaw_Kd);
}else if(c=='.'){
yaw_Kd = INITYAWKd;
Serial.print("yaw_Kd = ");Serial.println(yaw_Kd);
}
else if(c=='p'){
Serial_print_sig = !(Serial_print_sig);
}else if(c=='/'){
roll_print = !(roll_print);
}else if(c=='*'){
pitch_print = !(pitch_print);
}else if(c=='-'){
yaw_print = !(yaw_print);
}else if(c=='+'){
hover_print = !(hover_print);
}
}
//---------------------------------------------------------------------//
//
// main
//
//--------------------------------------------------------------------//
void setup() {
Serial.begin(115200);
SensorSerial.begin(57600);
callibration();
}
void loop() {
if(SensorSerial.available()){ //데이터 받기.
if(recieve_data()==1) //데이터 다 받음?
{
if(trans_data_to_degree(SBuf)==1) //센서값.
{
if(initial == 0){
yaw_setpoint = DegYaw;
initial = 1;
//Serial.println(yaw_setpoint);
}
rollControll = roll_pid(roll_setpoint, DegRoll); //PID.
pitchControll = pitch_pid(pitch_setpoint, DegPitch);
yawControll = yaw_pid(yaw_setpoint, DegYaw);
run_motors(hover, rollControll, pitchControll, yawControll); //MOTOR
//Serial_Monitor(); //Serial.print.
Serial.print(Acc_x);Serial.print(", ");Serial.print(Acc_y);Serial.print(", ");Serial.println(Acc_y);
}
}
}
if(Serial.available()){ //통신입력?
from_computer(); //입력받기.
}
}
Manual for WFT06X.pdf
