Amber Tang
20170830
Dependencies: MPU9250_SPI PID02 SDFileSystem02 Servo mbed
main.cpp
- Committer:
- Amber77
- Date:
- 2017-08-30
- Revision:
- 0:67bd1a7efc59
File content as of revision 0:67bd1a7efc59:
#include "mbed.h"
#include "PID.h"
#include "Servo.h"
#include "MPU9250.h"
#include "SDFileSystem.h"
/* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
/* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
// SDFileSystem---1. https://developer.mbed.org/users/mbed_official/code/SDFileSystem/
// 2. https://developer.mbed.org/users/simon/code/SDCardTest/
// SD care---3.3v
// Ctrl+? =>註解
#define pi 3.14159265359
//**************************** PID control para declaration ************************************//
#define RATE 0.005
#define Kp_1 1.1 //1.1
#define Ki_1 0.18 //0.2
#define Kd_1 0.001
//**************************** Angle Sonsor para declaration ************************************//
#define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases
//**************************** pragram debug declaration ************************************//
int index_times = 0;
double RunTime =0,lastTime =0;
double t_trajectory=0;
int do_measure_index=0;
double t_MeasureAngularVelocity=0.03;
double t_PIDcontrol_velocity =0.03;
double t_LQR_control = 0.05;
double t_MeasureRobotAttitudeAngle = 0.05;
double t_quadratureDecoder = 15;
//double t_TrajectoryTracking = 0.1;
//**************************** Motor para declaration ************************************//
double r_ball = 0.1; // 球的半徑
double r_grounder = 0.018; //滾球的半徑
//**************************** Ticker ************************************//
Serial pc(USBTX, USBRX);
Ticker Sample_Motor_encoder; // create a timer to sample the encoder.
Ticker Sample_robotAngleSensor; // create a timer to sample the robot attitude.
Ticker MeasureAngularVelocity; // create a timer to measure the motor angular velocity.
Ticker PIDcontrol_velocity; // create a timer to do the motor angular velocity PID control.
Ticker LQR_control; // create a timer to do the position control.
//Ticker TrajectoryTracking_control;
//**************************** Measuremant of angular velocity declaration ************************************//
double Angle_1 =0,Angle_2 =0,Angle_3 =0,Angle_4 =0;
double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0,LastAngle_4 =0;
double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4;
double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4;
double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4;
double _4_SectionAngle=0,_4_LastSectionAngle,_4_LastSectionAngle_1,_4_LastSectionAngle_2,_4_LastSectionAngle_3,_4_LastSectionAngle_4;
double Average_SectionAngle_1,Now_angularVelocity_1=0;
double Average_SectionAngle_2,Now_angularVelocity_2=0;
double Average_SectionAngle_3,Now_angularVelocity_3=0;
double Average_SectionAngle_4,Now_angularVelocity_4=0;
double Now_angularVelocity_X=0,Now_angularVelocity_Y=0;
double Angle_X=0, Angle_Y=0;
double SectionTime =0;
double NowTime_measureVelocity=0, LastTime_measureVelocity = 0;
//**************************** PID control declaration ************************************//
float Now_time_PID,Last_Time_PID;
//Command
//double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1,Command_AngularVel_4=1;
//double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1,command_AngularVel_4=1;
double Command_AngularVel_X=1,Command_AngularVel_Y=1;
double command_AngularVel_X=1,command_AngularVel_Y=1;
//Control
double Control_motor_X,Control_motor_Y;
//Control PWM value
double Control_Motor_PWM_X,Control_Motor_PWM_Y;
PID Motor_X (&Now_angularVelocity_X, &Control_motor_X, &Command_AngularVel_X, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
PID Motor_Y (&Now_angularVelocity_Y, &Control_motor_Y, &Command_AngularVel_Y, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
//PID Motor_X (&Command_AngularVel_X, &Control_motor_X, &Now_angularVelocity_X, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
//PID Motor_Y (&Command_AngularVel_Y, &Control_motor_Y, &Now_angularVelocity_Y, Kp_1, Ki_1, Kd_1, DIRECT,&Now_time_PID,&Last_Time_PID);
//****************************LQR_control declaration************************************//
double Ka = 60; //16.9573 25.0
double Kav = 10; //10.4423 10.7423
double Kt = 4.0 ; //3.0 3.1
double Kt_y = 4.0; //4.0 3.98
double Kv = 1.0; //1.0
double Kv_y = 1.1; //1.1
double Kz = 0.05; //0.05
double Kii = 0.2; //0.2
double KI_xy = 0.9; //0.02
double KI_xy_y = 0.9; //0.025
double Roll_offset = -5; //2.88 4.0 3.5
double Pitch_offset = -4; //-0.05 2.1 2.1
double Diff_Roll,Diff_Pitch,Diff_Yaw;
double Integ_Roll,Integ_Pitch;
double Integ_x,Integ_y;
double Roll_last,Pitch_last;
double Vx=0,Vy=0,Wz=0;
double d_x=0,d_y=0;
double u_x=0,u_y=0;
double Point_x;
double x_now,y_now;
double x_pre_1,y_pre_1;
double ax_now,ay_now;
double ax_pre_1,ay_pre_1;
double ax_pre_2,ay_pre_2;
double Vx_pre_1,Vy_pre_1;
double Diff_x,Diff_y;
double Diff_x_pre,Diff_y_pre;
double dot_diff_x,dot_diff_y;
//****************************Trajectory Tracking declaration************************************//
double x_trajectory=0,y_trajectory=0;
//****************************Encoder declaration************************************//
// phase a of the quadrature encoder
// phase b of the quadrature encoder
DigitalIn phaseA_1( PA_0 );
DigitalIn phaseB_1( PA_1 );
DigitalIn phaseA_2( PA_8 );
DigitalIn phaseB_2( PA_9 );
DigitalIn phaseA_3( PC_6 );
DigitalIn phaseB_3( PC_7 );
DigitalIn phaseA_4( PB_8 );
DigitalIn phaseB_4( PB_9 );
// hold the signed value corresponding to the number of clicks left or right since last sample
// keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
int encoderClickCount_1 = 0;
int previousEncoderState_1 = 0;
int encoderClickCount_2 = 0;
int previousEncoderState_2 = 0;
int encoderClickCount_3 = 0;
int previousEncoderState_3 = 0;
int encoderClickCount_4 = 0;
int previousEncoderState_4 = 0;
//****************************Angle Sensor declaration************************************//
float Times[10] = {0,0,0,0,0,0,0,0,0,0};
float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz
int counter = 0;
int divider = 20;
float dt; // time for entire loop
float dt_sensors; // time only to read sensors
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float OX = 0, OY = 0, OZ = 0;
float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4;
float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4;
float Mag_x_pre,Mag_y_pre,Mag_z_pre;
float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
float Global_mag_vector_angle,Yaw_pre;
float Global_mag_x_vector_angle,Mag_x_vector_angle;
float Global_mag_y_vector_angle,Mag_y_vector_angle;
int Count_mag_check=0;
float angle[3];
float Roll,Pitch,Yaw;
float calibrated_values[3],magCalibrationp[3];
float v_index[3];
float dest1,dest2;
int f=0;
int j=0;
int k=0;
int g=0;
int count1=0,count2=0,count3=0,count4=0,count5=0,count6=0,count7=0,count8=0,count9=0,count11=0,count12=0,count14=0;
int Rot_index;
float mRes = 10.*4912./32760.0;
int AngleFilter_counter = 0;
float Roll_total = 0,Pitch_total = 0;
//****************************Motor & SD card & MPU9250 declare************************************//
int control_AR=0, control_brake=0,control_stopRun=1, control_X1_CW_CCW=0, control_X2_CW_CCW=0, control_Y1_CW_CCW=0, control_Y2_CW_CCW=0;
int Control_stopRun=1, Control_brake=0, Control_AR=0, Control_X1_CW_CCW=0, Control_X2_CW_CCW=0, Control_Y1_CW_CCW=0, Control_Y2_CW_CCW=0;
int control_LiftingStopRun=1, control_F_R=1, control_H_F=1;
int Control_LiftingStopRun=1, Control_F_R=1, Control_H_F=1;
int X_pos=0, Y_pos=0;
DigitalOut StopRun(PA_12);
DigitalOut Brake(PA_11);
DigitalOut AR(PB_12);
DigitalOut X1_CW_CCW(PC_11);
DigitalOut X2_CW_CCW(PC_10);
DigitalOut Y1_CW_CCW(PC_12);
DigitalOut Y2_CW_CCW(PD_2);
Servo X_PWM(PB_1);
Servo Y_PWM(PB_2);
DigitalOut myled(LED1);
//---SD card---//
SDFileSystem sd( D4, D5, D3, D6, "sd"); // mosi, miso, sclk, cs
DigitalIn mybutton(USER_BUTTON);
//---MPU 9250---//
SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
//SPI_MOSI = PA_7(D11), SPI_MISO = PA_6(D12), SPI_SCK = PA_5(D13), SPI_CS = PB_6(D10)
mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object
//****************************Lifting mechanism & limit switches declare************************************//
DigitalOut LiftingStopRun(PA_15);
DigitalOut F_R(PA_14); // CW/CCW
DigitalOut H_F(PA_13);
//DigitalIn TIM(PB_7);
DigitalIn LimitSwitchUp(PB_15,PullUp);
DigitalIn LimitSwitchDown(PB_14,PullUp);
int limitSwitchUp;
int limitSwitchDown;
//****************************Timer declaration************************************//
Timer NowTime;
Timer LiftingDownTime;
//---------------------------------------------------------------------------------//
//**************************** Filter_IMUupdate ************************************//
//**************************** Filter_IMUupdate ************************************//
void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
if(norm == 0.0f) return;
ax /= norm;
ay /= norm;
az /= norm;
// estimated direction of gravity
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// error is sum of cross product between reference direction of field and direction measured by sensor
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// integral error scaled integral gain
exInt += ex*Ki;
eyInt += ey*Ki;
ezInt += ez*Ki;
// adjusted gyroscope measurements
gx += Kp*ex + exInt;
gy += Kp*ey + eyInt;
gz += Kp*ez + ezInt;
// integrate quaternion rate and normalise
float q0o = q0; // he did the MATLAB to C error by not thinking of the beginning vector elements already being changed for the calculation of the rest!
float q1o = q1;
float q2o = q2;
float q3o = q3;
q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//**************************** Filter_compute ************************************//
//**************************** Filter_compute ************************************//
void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
{
// IMU/AHRS
float d_Gyro_angle[3];
void get_Acc_angle(const float * Acc_data);
// IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
float radGyro[3],Gyro_cal_data; // Gyro in radians per second
for(int i=0; i<3; i++)
{
radGyro[i] = Gyro_data[i] * 3.14159/ 180;
}
Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
//IMU_AHRSupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2], Comp_data[0], Comp_data[1], Comp_data[2]);
float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
rangle[1] = asin (2*q0*q2 - 2*q3*q1);
rangle[2] = atan2(2*q0_A*q3_A + 2*q1_A*q2_A, 1 - 2*(q2_A*q2_A + q3_A*q3_A)); // Z-axis
for(int i=0; i<2; i++)
{ // angle in degree
angle[i] = rangle[i] * 180 / 3.14159;
}
/*Roll=angle[0]*0.2 + Roll_pre_1*0.2 + Roll_pre_2*0.2 +Roll_pre_3*0.2 + Roll_pre_4*0.2;
Roll_pre_4 = Roll_pre_3;
Roll_pre_3 = Roll_pre_2;
Roll_pre_2 = Roll_pre_1;
Roll_pre_1 = Roll;
Pitch=angle[1]*0.2 + Pitch_pre_1*0.2 + Pitch_pre_2*0.2 + Pitch_pre_3*0.2 + Pitch_pre_4*0.2;
Pitch_pre_4 = Pitch_pre_3;
Pitch_pre_3 = Pitch_pre_2;
Pitch_pre_2 = Pitch_pre_1;
Pitch_pre_1 = Pitch;*/
Roll_total += angle[0];
Pitch_total += angle[1];
AngleFilter_counter++;
if( AngleFilter_counter > 1 )
{ // The average of the attitude angle for 100 times.
Roll = Roll_total / AngleFilter_counter;
Pitch = Pitch_total / AngleFilter_counter;
AngleFilter_counter = 0;
Roll_total = 0;
Pitch_total = 0;
Roll -= Roll_offset;
Pitch -= Pitch_offset;
}
//**************************************************Gyro_data[2] filter start
float GYRO_z=0;
GYRO_z=Gyro_data[2]*0.15 + GYRO_z_pre*0.20 + GYRO_z_pre_L*0.20 + GYRO_z_pre_LL*0.25 + GYRO_z_pre_LLL*0.20;
if( count4==1 )
{
GYRO_z_pre_L=GYRO_z_pre;
count4=0;
}
if( count5==2 )
{
GYRO_z_pre_LL=GYRO_z_pre_L;
count5=0;
}
if( count6==3 )
{
GYRO_z_pre_LLL=GYRO_z_pre_LL;
count6=0;
}
count4++;
count5++;
count6++;
GYRO_z_pre=Gyro_data[2];
Global_GYRO_z=GYRO_z;
/*printf(" GYRO_z:%10.3f ,count8:%10d \n",
GYRO_z,
count8
);*/
if((count8>5)&&(count8<=2005))
{
GYRO_z_total+=GYRO_z;
}
if( count8==2005 )
{
GYRO_z_offset=GYRO_z_total/2000;
/* printf(" GYRO_z_offset:%10.5f \n ",
GYRO_z_offset
);*/
GYRO_z_total=0;
count8=0;
}
count8++;
//**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 0.74956
//************************************************** calculate Yaw
if( (count11==35) )
{
if( abs(Yaw_pre-Yaw)<1 )
{
Yaw_pre=Yaw_pre;
}
else
{
Yaw_pre=Yaw;
}
count11=0;
}
count11++;
if( count12>=20 )
{
Yaw += (Gyro_data[2]-0.74936) *dt;
}
count12++;
//pc.printf(" Yaw:%10.5f ",
//Yaw
// );
}
//**************************** Mag_Complentary_Filter ************************************//
void Mag_Complentary_Filter(float dt, const float * Comp_data)
{
float Mag_x=0,Mag_y=0,Mag_z=0;
Mag_x=Comp_data[0]*0.15 + Mag_x_pre*0.20 + Mag_x_pre_L*0.20 + Mag_x_pre_LL*0.25 + Mag_x_pre_LLL*0.20;
Mag_y=Comp_data[1]*0.15 + Mag_y_pre*0.20 + Mag_y_pre_L*0.20 + Mag_y_pre_LL*0.25 + Mag_y_pre_LLL*0.20;
Mag_z=Comp_data[2]*0.15 + Mag_z_pre*0.20 + Mag_z_pre_L*0.20 + Mag_z_pre_LL*0.25 + Mag_z_pre_LLL*0.20;
if( count1==1 )
{
Mag_x_pre_L=Mag_x_pre;
Mag_y_pre_L=Mag_y_pre;
Mag_z_pre_L=Mag_z_pre;
Cal_Mag_x_pre=Cal_Mag_x;
count1=0;
}
if( count2==2 )
{
Mag_x_pre_LL=Mag_x_pre_L;
Mag_y_pre_LL=Mag_y_pre_L;
Mag_z_pre_LL=Mag_z_pre_L;
Cal_Mag_x_pre_L=Cal_Mag_x_pre;
count2=0;
}
if( count7==3 )
{
Mag_x_pre_LLL=Mag_x_pre_LL;
Mag_y_pre_LLL=Mag_y_pre_LL;
Mag_z_pre_LLL=Mag_z_pre_LL;
Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
count7=0;
}
count1++;
count2++;
count7++;
Mag_x_pre=Comp_data[0];
Mag_y_pre=Comp_data[1];
Mag_z_pre=Comp_data[2];
if( count14>4 )
{
Cal_Mag_x=Mag_x;
}
count14++;
//*************************************Mag_ave calculate
if(count3<=20)
{
Mag_x_total+=Mag_x;
Mag_y_total+=Mag_y;
}
if( count3==20)
{
Mag_x_ave=Mag_x_total/21;
Mag_y_ave=Mag_y_total/21;
/*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ",
Mag_x_ave,
Mag_y_ave
);*/
Mag_x_total=0;
Mag_y_total=0;
count3=0;
}
count3++;
//********************************ROT_check start
float v_length,v_length_ave,MagVector_angle;
v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y );
v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave );
MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3;
if( count9==3 )
{
Global_mag_vector_angle=MagVector_angle;
count9=0;
}
count9++;
if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) )
{
Count_mag_check++;
}
else
{
Count_mag_check=0;
}
if( Count_mag_check==30 )
{
Yaw=Yaw_pre;
Count_mag_check=0;
}
float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
//********************************Theta_check end
/*pc.printf("ABS_CHECK:%10.3f,Cal_Mag_x_pre_LL:%10.3f,Mag_x:%10.3f,Count_mag_check:%10d ,Yaw_pre:%10.3f,Yaw_filter:%10.3f ",
ABS_CHECK,
Cal_Mag_x_pre_LL,
Mag_x,
Count_mag_check,
Yaw_pre,
Yaw
);*/
}
//****************************Motor_run************************************//
int Motor_run(double control_value_PWM_X, double control_value_PWM_Y, int control_AR, int control_brake , int control_stopRun, int control_X1_CW_CCW, int control_X2_CW_CCW, int control_Y1_CW_CCW, int control_Y2_CW_CCW)
{
StopRun.write(control_stopRun);
Brake.write(control_brake);
AR.write(control_AR);
X1_CW_CCW.write(control_X1_CW_CCW);
X2_CW_CCW.write(control_X2_CW_CCW);
Y1_CW_CCW.write(control_Y1_CW_CCW);
Y2_CW_CCW.write(control_Y2_CW_CCW);
X_PWM.write(control_value_PWM_X);
Y_PWM.write(control_value_PWM_Y);
}
//****************************quadratureDecoder************************************//
void quadratureDecoder( void )
{
int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read();
int currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read();
int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read();
int currentEncoderState_4 = (phaseB_4.read() << 1) + phaseA_4.read();
//**************************** 1 ************************************//
if( currentEncoderState_1 == previousEncoderState_1 )
{
return;
}
switch( previousEncoderState_1 )
{
case 0:
if( currentEncoderState_1 == 2 )
{
encoderClickCount_1--;
}
else if( currentEncoderState_1 == 1 )
{
encoderClickCount_1++;
}
break;
case 1:
if( currentEncoderState_1 == 0 )
{
encoderClickCount_1--;
}
else if( currentEncoderState_1 == 3 )
{
encoderClickCount_1++;
}
break;
case 2:
if( currentEncoderState_1 == 3 )
{
encoderClickCount_1--;
}
else if( currentEncoderState_1 == 0 )
{
encoderClickCount_1++;
}
break;
case 3:
if( currentEncoderState_1 == 1 )
{
encoderClickCount_1--;
}
else if( currentEncoderState_1 == 2 )
{
encoderClickCount_1++;
}
break;
default:
break;
}
previousEncoderState_1 = currentEncoderState_1;
//**************************** 2 ************************************//
if( currentEncoderState_2 == previousEncoderState_2 )
{
return;
}
switch( previousEncoderState_2 )
{
case 0:
if( currentEncoderState_2 == 2 )
{
encoderClickCount_2--;
}
else if( currentEncoderState_2 == 1 )
{
encoderClickCount_2++;
}
break;
case 1:
if( currentEncoderState_2 == 0 )
{
encoderClickCount_2--;
}
else if( currentEncoderState_2 == 3 )
{
encoderClickCount_2++;
}
break;
case 2:
if( currentEncoderState_2 == 3 )
{
encoderClickCount_2--;
}
else if( currentEncoderState_2 == 0 )
{
encoderClickCount_2++;
}
break;
case 3:
if( currentEncoderState_2 == 1 )
{
encoderClickCount_2--;
}
else if( currentEncoderState_2 == 2 )
{
encoderClickCount_2++;
}
break;
default:
break;
}
previousEncoderState_2 = currentEncoderState_2;
//**************************** 3 ************************************//
if( currentEncoderState_3 == previousEncoderState_3 )
{
return;
}
switch( previousEncoderState_3 )
{
case 0:
if( currentEncoderState_3 == 2 )
{
encoderClickCount_3--;
}
else if( currentEncoderState_3 == 1 )
{
encoderClickCount_3++;
}
break;
case 1:
if( currentEncoderState_3 == 0 )
{
encoderClickCount_3--;
}
else if( currentEncoderState_3 == 3 )
{
encoderClickCount_3++;
}
break;
case 2:
if( currentEncoderState_3 == 3 )
{
encoderClickCount_3--;
}
else if( currentEncoderState_3 == 0 )
{
encoderClickCount_3++;
}
break;
case 3:
if( currentEncoderState_3 == 1 )
{
encoderClickCount_3--;
}
else if( currentEncoderState_3 == 2 )
{
encoderClickCount_3++;
}
break;
default:
break;
}
previousEncoderState_3 = currentEncoderState_3;
//**************************** 4 ************************************//
if( currentEncoderState_4 == previousEncoderState_4 )
{
return;
}
switch( previousEncoderState_4 )
{
case 0:
if( currentEncoderState_4 == 2 )
{
encoderClickCount_4--;
}
else if( currentEncoderState_4 == 1 )
{
encoderClickCount_4++;
}
break;
case 1:
if( currentEncoderState_4 == 0 )
{
encoderClickCount_4--;
}
else if( currentEncoderState_4 == 3 )
{
encoderClickCount_4++;
}
break;
case 2:
if( currentEncoderState_4 == 3 )
{
encoderClickCount_4--;
}
else if( currentEncoderState_4 == 0 )
{
encoderClickCount_4++;
}
break;
case 3:
if( currentEncoderState_4 == 1 )
{
encoderClickCount_4--;
}
else if( currentEncoderState_4 == 2 )
{
encoderClickCount_4++;
}
break;
default:
break;
}
previousEncoderState_4 = currentEncoderState_4;
}
//****************************getAngular************************************//
//****************************getAngular************************************//
void getAngular( void )
{
// Angle_1 = (encoderClickCount_1*0.1499)/5;
// Angle_2 = (encoderClickCount_2*0.1499)/5;
// Angle_3 = (encoderClickCount_3*0.1499)/5;
// Angle_4 = (encoderClickCount_4*0.1499)/5;
Angle_1 = encoderClickCount_1*0.03;
Angle_2 = encoderClickCount_2*0.03;
Angle_3 = encoderClickCount_3*0.03;
Angle_4 = encoderClickCount_4*0.03;
_1_SectionAngle = Angle_1 - LastAngle_1;
_2_SectionAngle = Angle_2 - LastAngle_2;
_3_SectionAngle = Angle_3 - LastAngle_3;
_4_SectionAngle = Angle_4 - LastAngle_4;
Average_SectionAngle_1 = (_1_SectionAngle*0.3 + _1_LastSectionAngle*0.3 + _1_LastSectionAngle_1*0.1 + _1_LastSectionAngle_2*0.1 + _1_LastSectionAngle_3*0.1 + _1_LastSectionAngle_4*0.1)/1;
Average_SectionAngle_2 = (_2_SectionAngle*0.3 + _2_LastSectionAngle*0.3 + _2_LastSectionAngle_1*0.1 + _2_LastSectionAngle_2*0.1 + _2_LastSectionAngle_3*0.1 + _2_LastSectionAngle_4*0.1)/1;
Average_SectionAngle_3 = (_3_SectionAngle*0.3 + _3_LastSectionAngle*0.3 + _3_LastSectionAngle_1*0.1 + _3_LastSectionAngle_2*0.1 + _3_LastSectionAngle_3*0.1 + _3_LastSectionAngle_4*0.1)/1;
Average_SectionAngle_4 = (_4_SectionAngle*0.3 + _4_LastSectionAngle*0.3 + _4_LastSectionAngle_1*0.1 + _4_LastSectionAngle_2*0.1 + _4_LastSectionAngle_3*0.1 + _4_LastSectionAngle_4*0.1)/1;
NowTime_measureVelocity = NowTime.read();
SectionTime = NowTime_measureVelocity - LastTime_measureVelocity;
Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime));
Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime));
Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime));
Now_angularVelocity_4 = abs(Average_SectionAngle_4/(SectionTime));
Now_angularVelocity_X = (Now_angularVelocity_2+Now_angularVelocity_4)*0.5;
Now_angularVelocity_Y = (Now_angularVelocity_1+Now_angularVelocity_3)*0.5;
LastTime_measureVelocity = NowTime_measureVelocity;
LastAngle_1 = Angle_1;
_1_LastSectionAngle_4 = _1_LastSectionAngle_3;
_1_LastSectionAngle_3 = _1_LastSectionAngle_2;
_1_LastSectionAngle_2 = _1_LastSectionAngle_1;
_1_LastSectionAngle_1 = _1_LastSectionAngle;
_1_LastSectionAngle = _1_SectionAngle;
LastAngle_2 = Angle_2;
_2_LastSectionAngle_4 = _2_LastSectionAngle_3;
_2_LastSectionAngle_3 = _2_LastSectionAngle_2;
_2_LastSectionAngle_2 = _2_LastSectionAngle_1;
_2_LastSectionAngle_1 = _2_LastSectionAngle;
_2_LastSectionAngle = _2_SectionAngle;
LastAngle_3 = Angle_3;
_3_LastSectionAngle_4 = _3_LastSectionAngle_3;
_3_LastSectionAngle_3 = _3_LastSectionAngle_2;
_3_LastSectionAngle_2 = _3_LastSectionAngle_1;
_3_LastSectionAngle_1 = _3_LastSectionAngle;
_3_LastSectionAngle = _3_SectionAngle;
LastAngle_4 = Angle_4;
_4_LastSectionAngle_4 = _4_LastSectionAngle_3;
_4_LastSectionAngle_3 = _4_LastSectionAngle_2;
_4_LastSectionAngle_2 = _4_LastSectionAngle_1;
_4_LastSectionAngle_1 = _4_LastSectionAngle;
_4_LastSectionAngle = _4_SectionAngle;
if (Angle_1>=0 && Angle_3>=0)
{
Angle_Y = (Angle_1 + Angle_3)*0.5;
}
else if(Angle_1>=0 && Angle_3<=0)
{
Angle_Y = (Angle_1 + abs(Angle_3))*0.5;
}
else if(Angle_1<=0 && Angle_3>=0)
{
Angle_Y = -(abs(Angle_1) + Angle_3)*0.5;
}
else if(Angle_1<=0 && Angle_3<=0)
{
Angle_Y = (Angle_1 + Angle_3)*0.5;
}
if (Angle_2>=0 && Angle_4>=0)
{
Angle_X = (Angle_2 + Angle_4)*0.5;
}
else if (Angle_2>=0 && Angle_4<=0)
{
Angle_X = (Angle_2 + abs(Angle_4))*0.5;
}
else if (Angle_2<=0 && Angle_4>=0)
{
Angle_X = -(abs(Angle_2) + Angle_4)*0.5;
}
else if (Angle_2<=0 && Angle_4<=0)
{
Angle_X = (Angle_2 + Angle_4)*0.5;
}
}
//****************************PWM_commmand_transformation************************************//
//****************************PWM_commmand_transformation************************************//
double PWM_commmand_transformation( double Control_AngVel_Value )
{
double Control_PWM_Value = 0;
if( Control_AngVel_Value > 0 )
{
Control_AngVel_Value = Control_AngVel_Value;
}
else if( Control_AngVel_Value < 0 )
{
Control_AngVel_Value = -Control_AngVel_Value;
}
if( Control_AngVel_Value > 325)
{
if( Control_AngVel_Value < 466 )
{
if( Control_AngVel_Value < 393 )
{
Control_PWM_Value = Control_AngVel_Value/651.6 ; //0.5~0.6
}
else
{
Control_PWM_Value = Control_AngVel_Value/658.8; //0.6~0.7
}
}
else
{
if( Control_AngVel_Value < 523 )
{
Control_PWM_Value = Control_AngVel_Value/658.39; //0.7~0.8
}
else
{
if( Control_AngVel_Value < 588 )
{
Control_PWM_Value = Control_AngVel_Value/652.36; //0.8~0.9
}
else
{
Control_PWM_Value = Control_AngVel_Value/655.11; //0.9~1
}
}
}
}
else if( Control_AngVel_Value < 325)
{
if( Control_AngVel_Value < 40 )
{
Control_PWM_Value = Control_AngVel_Value/533.3; //0~0.075
}
else
{
if( Control_AngVel_Value < 59 )
{
Control_PWM_Value = Control_AngVel_Value/560.65; //0.1~0.15
}
else
{
if( Control_AngVel_Value < 131 )
{
Control_PWM_Value = Control_AngVel_Value/638.3; //0.15~0.2
}
else
{
if( Control_AngVel_Value < 197 )
{
Control_PWM_Value = Control_AngVel_Value/651.6; //0.2~0.3
}
else
{
if( Control_AngVel_Value < 263 )
{
Control_PWM_Value = Control_AngVel_Value/654.16; //0.3~0.4
}
else
{
Control_PWM_Value = Control_AngVel_Value/652.5; //0.4~0.5
}
}
}
}
}
}
return Control_PWM_Value;
}
//****************************PIDcontrol_compute_velocity************************************//
//****************************PIDcontrol_compute_velocity************************************//
void PIDcontrol_compute_velocity(void)
{
if(command_AngularVel_X >= 0)
{
Control_X1_CW_CCW = 1;
Control_X2_CW_CCW = 0;
Command_AngularVel_X = command_AngularVel_X;
}
else
{
Control_X1_CW_CCW = 0;
Control_X2_CW_CCW = 1;
Command_AngularVel_X = -command_AngularVel_X;
}
if(command_AngularVel_Y >= 0)
{
Control_Y1_CW_CCW = 1;
Control_Y2_CW_CCW = 0;
Command_AngularVel_Y = command_AngularVel_Y;
}
else
{
Control_Y1_CW_CCW = 0;
Control_Y2_CW_CCW = 1;
Command_AngularVel_Y = -command_AngularVel_Y;
}
Now_time_PID=NowTime.read();
Motor_X.Compute(&Now_time_PID);
Motor_Y.Compute(&Now_time_PID);
// Control_Motor_PWM_X = PWM_commmand_transformation(Control_motor_X);
// Control_Motor_PWM_Y = PWM_commmand_transformation(Control_motor_Y);
Control_Motor_PWM_X = PWM_commmand_transformation(command_AngularVel_X);
Control_Motor_PWM_Y = PWM_commmand_transformation(command_AngularVel_Y);
}
//****************************Compute_point************************************//
//****************************Compute_point************************************//
void LQR_control_compute(void)
{
Diff_Roll = (Roll - Roll_last);
Diff_Pitch = (Pitch - Pitch_last);
Integ_Roll += Roll;
//printf("Diff_Roll:%.3f\n",Diff_Roll);
Integ_Pitch += Pitch;
Roll_last = Roll;
Pitch_last = Pitch;
//Diff_Roll =0;
//Roll -= 2.45;
//Pitch -=2.5;
Diff_x = x_now - x_trajectory;
Diff_y = y_now - y_trajectory;
dot_diff_x = Diff_x - Diff_x_pre;
dot_diff_y = Diff_y - Diff_y_pre;
Integ_x += Diff_x;
Integ_y += Diff_y;
//x_pre_1 = x_now;
//y_pre_1 = y_now;
Diff_x_pre = Diff_x;
Diff_y_pre = Diff_y;
// u_x = Ka*(Roll)+ Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x;
// u_y = Ka*(Pitch)+Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y;
u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll + Kt*Diff_x + Kv*dot_diff_x + KI_xy*Integ_x;
// u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll;
//u_x = Ka*(Roll)+(Integ_Roll*Kii) + Kav*Diff_Roll - Kt*(10) - Kv*0;
// u_x = Ka*(Roll);
u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch + Kt_y*Diff_y + Kv_y*dot_diff_y + KI_xy_y*Integ_y;
// u_y = Ka*(Pitch)+(Integ_Roll*Kii) + Kav*Diff_Roll;
//u_y = Ka*(Pitch)+(Integ_Pitch*Kii) + Kav*Diff_Pitch - Kt*0 - Kv*0;
// u_y = Ka*(Pitch);
if(u_x > 580)
{
u_x = 580;
}
else if(u_x < -580)
{
u_x = -580;
}
if(u_y > 580 )
{
u_y = 580;
}
else if(u_y < -580)
{
u_y = -580;
}
Vx = u_x;
Vy = u_y;
Wz = Yaw;
// Vx = 100;
// Vy = 100;
// Wz =0;
command_AngularVel_X = Vx+Kz*Wz;
command_AngularVel_Y = Vy+Kz*Wz;
}
//***********************************************************//
//
////若要前進，則在x或y加減p_set。根據底盤的正負標識，來決定加或減。
//
//
// /*v_dx_new=0.4*(1*Kp3*(err_tiltX)+(1.9*Kd3*1)*(err_gyroX) +(x)*Kp3*19 +vel_x*Kd3*15-0*v_dx);
// v_dy_new=0.4*(1*Kp3*(err_tiltY)+(1.9*Kd3*1)*(err_gyroY) +(y)*Kp3*19 +(vel_y)*Kd3*15-0*v_dy);*/
// // x_u=12.5*1.6666*v_d_gain*(1*Kp3*(err_tiltX)+(1.9*Kd3*1)*(err_gyroX) +(x)*Kp3*0 +vel_x*Kd3*0-0*x_iu);
// // y_u=12.5*1.6666*v_d_gain*(1*Kp3*(err_tiltY)+(1.9*Kd3*1)*(err_gyroY) +(y)*Kp3*0 +vel_y*Kd3*0-0*y_iu);
// /*x_u=6.25*v_d_gain*( -Kp3*err_tiltX+(-1.9*Kd3)*err_gyroX +(-0.001*0*Kp3)*x +(-0.018*0*Kp3)*vel_x-0*x_iu);
// y_u=6.25*v_d_gain*( -Kp3*err_tiltY+(-1.9*Kd3)*err_gyroY +(-0.001*0*Kp3)*y +(-0.018*0*Kp3)*vel_y-0*y_iu);
//*/
// // integral U for PI close loop
// x_iu=x_u*T2+x_iu_old; //integral value of v_dx
// y_iu=y_u*T2+y_iu_old;
//
// x_iu_old=x_iu;
// y_iu_old=y_iu;
//
// // dV cmd(U cmd) PI close loop (like low-pass filter multip a gain value)
// C_theta1_d=(8)*((x_u-vel_x*0.3*C_theta_gain1)+(C_theta_gain2)*(x_iu*0.24- x*0.3 )+fc*vel_x);
// C_theta2_d=(8)*((y_u-vel_y*0.3*C_theta_gain1)+(C_theta_gain2)*(y_iu*0.24- y*0.3 )+fc*vel_y);
//
// // Vx = integral dVx
//
// C_theta1=C_theta1_d*T2+C_theta1_old;
// // limit
// C_theta1= C_theta1> (5000) ? (5000): C_theta1;
// C_theta1= C_theta1< (-5000) ? (-5000): C_theta1;
//
// C_theta1_old=C_theta1;
// // integral Vx
// C_theta1_i=C_theta1*T2+C_theta1_i;
//
// // Vy = integral dVy
// C_theta2=C_theta2_d*T2+C_theta2_old;
// // limit
// C_theta2= C_theta2>(5000) ? (5000): C_theta2;
// C_theta2= C_theta2<(-5000) ? (-5000):C_theta2;
//
// C_theta2_old=C_theta2;
// // integral Vy
// C_theta2_i=C_theta2*T2+C_theta2_i;
//
// // speed PI LOOP control
//
// C_thetaX=Kcp*(C_theta1-vel_x*Cgain1)+Kci*(C_theta1_i-x*Cgain2); //X axis
// C_thetaY=Kcp*(C_theta2-vel_y*Cgain1)+Kci*(C_theta2_i-y*Cgain2); //Y axis
//
// #endif
// #if Testmode == 1 // nonlinear
//
////########### x 方向之控制器#############
// r1=1;
// r2=1;
// err_tilt= 0+tilt2*0.0006086;/*tilt_out;傾斜儀資料 */
// err_gyro= 0+gyro2;/*陀螺儀資料*/
// sin_t=sin(err_tilt);
// cos_t=cos(err_tilt);
// err_tilt_i=err_tilt_i+err_tilt*0.1;
// x_i=x_i+x*0.001;
// //err_tilt_i+=err_tilt;
// //x_i+=x;
// delta=187.5-125.3*cos_t;
// A=(5.51+33.75*cos_t)/delta;
// B=-((125.3*sin_t*cos_t)/delta)*err_gyro*err_gyro+182.2*sin_t/delta;
//// B=-((125.3*sin_t*cos_t)/delta)*err_gyro*err_gyro+182.2*sin_t/delta-5.5091*((err_gyro+vel_x/0.11))/187.5-125.3*cos_t*cos_t;
// C=Kp3*(eta1-Kp3*err_tilt-Ki3*err_tilt_i)+Ki3*err_tilt;
// D=-(5.51+33.75*cos_t)/delta;
// E=126.4/delta*err_gyro*err_gyro*sin_t-1637/delta*sin_t*cos_t;
// F=Kp3*(eta2-Kp3*x-Ki3*x_i)+Ki3*x;
////F=Kp3*(eta2-Kp3*err_tilt-Ki3*0)+Ki3*0;
// eta1=err_gyro+Kp3*err_tilt+Ki3*err_tilt_i;
// eta2=vel_x+Kp3*x+Ki3*x_i;
////eta2=0;
// s=r1*eta1+r2*eta2;
// if(s>=1) sgns=1;
// else if(s<=-1) sgns=-1;
// else sgns=s;
//
// if (s*abs(vel_x)>=1) sgns_c=1;
// else if(s*abs(vel_x)<=-1) sgns_c=-1;
// else sgns_c=s*abs(vel_x);
////########### y 方向之控制器#############
// err_tilt2= 0+tilt5*0.0006086;/*tilt_out;傾斜儀資料 */
// err_gyro2= 0+gyro5;/*陀螺儀資料*/
// sin_t2=sin(err_tilt2);
// cos_t2=cos(err_tilt2);
// err_tilt2_i=err_tilt2_i+err_tilt2*0.1;
// y_i=y_i+y*0.001;
////err_tilt2_i+=err_tilt2;
////y_i+=y;
// delta2=187.5-125.3*cos_t2;
// A2=(5.51+33.75*cos_t2)/delta2;
// B2=-((125.3*sin_t2*cos_t2)/delta2)*err_gyro2*err_gyro2+182.2*sin_t2/delta2;
// C2=Kp3*(eta3-Kp3*err_tilt2-Ki3*err_tilt2_i)+Ki3*err_tilt2;
// D2=-(5.51+33.75*cos_t2)/delta2;
// E2=126.4/delta2*err_gyro2*err_gyro2*sin_t2-1637/delta2*sin_t2*cos_t2;
// F2=Kp3*(eta4-Kp3*y-Ki3*y_i)+Ki3*y;
////F2=Kp3*(eta4-Kp3*err_tilt2-Ki3*0)+Ki3*0;
// eta3=err_gyro2+Kp3*err_tilt2+Ki3*err_tilt2_i;
// eta4=vel_y+Kp3*y+Ki3*y_i;
////eta4=0;
// s2=r1*eta3+r2*eta4;
// if(s2>=1) sgns2=1;
// else if(s2<=-1) sgns2=-1;
// else sgns2=s2;
//
// if(s2*abs(vel_y)>=1) sgns2_c=1;
// else if(s2*abs(vel_y)<=-1) sgns2_c=-1;
// else sgns2_c=s2*abs(vel_y);
//
////若要前進，則在x或y加減p_set。根據底盤的正負標識，來決定加或減。
// C_theta1_new=((r1*B+r1*C+r2*E+r2*F)+s+sgns+sgns_c)/(r1*A+r2*D);
// C_theta2_new=((r1*B2+r1*C2+r2*E2+r2*F2)+s2+sgns2+sgns2_c)/(r1*A2+r2*D2);
// }
// i++;
//
// C_theta1=C_theta1_new*0.00050+C_theta1_old;
// C_theta1_old=C_theta1;
//
//
// C_theta2=C_theta2_new*0.00050+C_theta2_old;
// C_theta2_old=C_theta2;
//
// C_theta1= C_theta1>(42143)?(42143): C_theta1;
// C_theta1= C_theta1<-1*(42143)?-1*(42143): C_theta1;
// C_theta2= C_theta2>(42143)?(42143): C_theta2;
// C_theta2=C_theta2<-1*(42143)?-1*(42143):C_theta2;
//
// #endif
//***********************************************************//
void MeasureRobotAttitudeAngle(void)
{
dt = t_MeasureRobotAttitudeAngle;
imu.read_all();
Mag_Complentary_Filter(dt,imu.Magnetometer);
Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
/*x_now = x_pre_1 + (1/(dt*dt))*(ax_now-2*ax_pre_1+ax_pre_2);
y_now = y_pre_1 + (1/(dt*dt))*(ay_now-2*ay_pre_1+ay_pre_2);
x_pre_1 = x_now;
y_pre_1 = y_now;*/
x_now = r_grounder*pi*(Angle_X/180);
y_now = r_grounder*pi*(Angle_Y/180);
// x_now = r_ball*pi*(Angle_X/180);
// y_now = r_ball*pi*(Angle_Y/180);
do_measure_index++;
}
//void Trajectory_tracking(void)
//{
// double t_trajectory = NowTime.read();
// if( RunTime>=0 &&( RunTime<=10))
// {
// //x_trajectory = t_trajectory * (0.5);
// x_trajectory = 0;
// y_trajectory = 0;
// }
// else if( RunTime>10 )
// {
// x_trajectory = 0;
// y_trajectory = 0;
// }
//}
//****************************main function************************************//
int main()
{
// pc.baud(115200);
pc.baud(230400);
//****************************Motor driver declare************************************//
//ExtInt = 0; //number high, voltage high.
//Control_stopRun=0;
// Control_brake=1;
Control_AR=1;
Control_H_F=1;
H_F.write(Control_H_F); // control_H_F must equal to 1
// StopRun.write(Control_stopRun);
// Brake.write(Control_brake);
// AR.write(Control_AR);
//****************************Angle Sensor initialization************************************//
if(imu.init(1,BITS_DLPF_CFG_188HZ))
{ //INIT the mpu9250
// pc.printf("\nCouldn't initialize MPU9250 via SPI!");
}
imu.whoami();
// wait(0.1);
imu.set_gyro_scale(BITS_FS_2000DPS);
// wait(0.1);
imu.set_acc_scale(BITS_FS_16G);
// wait(0.1);
imu.AK8963_whoami();
// wait(0.1);
imu.AK8963_calib_Magnetometer();
////****************************SD card************************************//
mkdir("/sd/Amber20170822a", 0777); //SD裡面的資料夾叫Amber77,在此做宣告
FILE *fp = fopen("/sd/Amber20170822a/RollPitchYaw_y1.csv", "a");
//將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv
fprintf(fp,"RunTime,Roll,Pitch,Yaw,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory\n");
//**************************** PWM ************************************//
// X_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
// Y_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
// X_PWM.calibrate(0.02, 0*0.02, 1*0.02);
// Y_PWM.calibrate(0.02, 0*0.02, 1*0.02);
X_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ
Y_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ
//**************************** Encoder ************************************//
phaseA_1.mode( PullUp );
phaseB_1.mode( PullUp );
phaseA_2.mode( PullUp );
phaseB_2.mode( PullUp );
phaseA_3.mode( PullUp );
phaseB_3.mode( PullUp );
phaseA_4.mode( PullUp );
phaseB_4.mode( PullUp );
//**************************** Create the motor encoder sampler. ************************************//
Sample_Motor_encoder.attach_us( &quadratureDecoder, t_quadratureDecoder );
//**************************** Create the motor encoder sampler. ************************************//
Sample_robotAngleSensor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle );
//**************************** Create the motor angular velocity measurement. ************************************//
MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity );
//**************************** Create the motor angular velocity PID control. ************************************//
PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity );
//**************************** Create the robot LQR control. ************************************//
LQR_control.attach( &LQR_control_compute, t_LQR_control );
//**************************** Trajectory tracking control ************************************//
// TrajectoryTracking_control.attach( &Trajectory_tracking, t_trajectory );
//**************************** Motor settlement ************************************//
Motor_X.SetMode(AUTOMATIC);
Motor_Y.SetMode(AUTOMATIC);
// Command_AngularVel_X = 0;
// Command_AngularVel_Y = 0;
//**************************** Timers start ************************************//
NowTime.start();
while( 1 )
{
RunTime = NowTime.read();
t_trajectory = NowTime.read();
limitSwitchUp = LimitSwitchUp.read();
limitSwitchDown = LimitSwitchDown.read();
//pc.printf("%d \n",Control_F_R);
if (mybutton == 0)
{
if( Control_F_R == 1)
{
Control_LiftingStopRun=0;
LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop
F_R.write(Control_F_R); // 0:turn down 1:turn up
Control_stopRun=0;
Control_brake=1;
Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
wait(3);
}
else if ( Control_F_R == 0)
{
Control_LiftingStopRun=0;
LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop
F_R.write(Control_F_R); // 0:turn down 1:turn up
Control_stopRun=0;
Control_brake=1;
Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
wait(2);
Control_stopRun=1;
Control_brake=1;
Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
wait(1);
}
Control_F_R=1-Control_F_R;
wait(0.2);
}
if(limitSwitchUp == 0)
{
LiftingStopRun.write(1); // 0:Run 1:Stop
F_R.write(1); // 0:turn down 1:turn up
}
if(limitSwitchDown == 0)
{
LiftingStopRun.write(1); // 0:Run 1:Stop
F_R.write(0); // 0:turn down 1:turn up
}
if( RunTime>=0 &&( RunTime<=10))
{
//x_trajectory = t_trajectory * (0.5);
//Control_stopRun=0;
// Control_brake=1;
x_trajectory = 0;
y_trajectory = 0;
}
else if( RunTime>10 && (RunTime <=20 ))
{
//Control_stopRun=0;
// Control_brake=1;
x_trajectory = (t_trajectory-10) * (0.05);
y_trajectory = 0;
}
else if( RunTime>=20 )
{
//Control_stopRun=0;
// Control_brake=1;
x_trajectory = 0;
y_trajectory = 0;
}
if( (RunTime-lastTime) > 0.1 )
{
index_times++;
lastTime = RunTime;
}
if( index_times >= 0.1 )
{
/*pc.printf("x_now: %.3f , y_now: %.3f , x_trajectory: %f , u_y: %f ",x_now,y_now,x_trajectory,u_y);
pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f \n",
Roll,
Pitch,
Yaw
);*/
//pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory);
//pc.printf("RunTime=%10.3f || Roll=%10.3f || Pitch=%10.3f || Yaw=%10.3f \n",RunTime, Roll, Pitch, Yaw);
// pc.printf("Vx=%10.3f || Vy=%10.3f || PWM_X=%10.3f || PWM_Y=%10.3f || x_now=%10.3f || y_now=%10.3f || Angle_X=%10.3f || Angle_Y=%10.3f\n",
// Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_now,y_now,Angle_X,Angle_Y);
pc.printf("RunTime=%.3f || Roll=%.3f || Pitch=%.3f || Yaw=%.3f \n",RunTime, Roll, Pitch, Yaw);
pc.printf("Vx=%.3f || Vy=%.3f || PWM_X=%.3f || PWM_Y=%.3f || x_now=%.3f || y_now=%.3f || Angle_X=%.3f || Angle_Y=%.3f\n\n\n",
Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory,Angle_X,Angle_Y);
//pc.printf(" Now_angularVelocity : %.3f ",Now_angularVelocity);
// pc.printf(",RunTime : %.3f ", RunTime );
// pc.printf(",control_PWM_Value : %.3f \n", control_PWM_Value);
index_times = 0;
//if(fp == NULL)
// {
// error("Could not open file for write\n");
// }
// //fprintf(fp,"%.3f,%.3f,%.3f\n", RunTime,Now_angularVelocity,control_PWM_Value);
fprintf(fp,"%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f \n", RunTime,Roll,Pitch,Yaw,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y, x_trajectory,y_trajectory);
}
// Lifting(Control_LiftingStopRun, Control_F_R , Control_H_F);
Motor_run(Control_Motor_PWM_X,Control_Motor_PWM_Y,Control_AR,Control_brake,Control_stopRun,Control_X1_CW_CCW,Control_X2_CW_CCW,Control_Y1_CW_CCW,Control_Y2_CW_CCW);
//wait(0.1);
// if(!mybutton)
// {
// StopRun.write(1);
// Brake.write(1);
// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
// X2_CW_CCW.write(0);
// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
// Y2_CW_CCW.write(0);
// X_PWM.write(0); // clockwise:0 counterclockwise:1
// Y_PWM.write(0);
// break;
// }
}
fclose(fp);
}