testSSWMR_TrajectoryTracking_OK - 20170830

Users » Amber77 » Code » testSSWMR_TrajectoryTracking_OK

Amber Tang / Mbed 2 deprecated testSSWMR_TrajectoryTracking_OK

20170830

Dependencies: MPU9250_SPI PID02 SDFileSystem02 Servo mbed

main.cpp@0:67bd1a7efc59, 2017-08-30 (annotated)

Committer:: Amber77
Date:: Wed Aug 30 02:10:02 2017 +0000
Revision:: 0:67bd1a7efc59

20170830

Who changed what in which revision?

User	Revision	Line number	New contents of line
Amber77	0:67bd1a7efc59	1	#include "mbed.h"
Amber77	0:67bd1a7efc59	2	#include "PID.h"
Amber77	0:67bd1a7efc59	3	#include "Servo.h"
Amber77	0:67bd1a7efc59	4	#include "MPU9250.h"
Amber77	0:67bd1a7efc59	5	#include "SDFileSystem.h"
Amber77	0:67bd1a7efc59	6	/* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
Amber77	0:67bd1a7efc59	7	/* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
Amber77	0:67bd1a7efc59	8	// SDFileSystem---1. https://developer.mbed.org/users/mbed_official/code/SDFileSystem/
Amber77	0:67bd1a7efc59	9	// 2. https://developer.mbed.org/users/simon/code/SDCardTest/
Amber77	0:67bd1a7efc59	10	// SD care---3.3v
Amber77	0:67bd1a7efc59	11	// Ctrl+? =>註解
Amber77	0:67bd1a7efc59	12	#define pi 3.14159265359
Amber77	0:67bd1a7efc59	13	//************************** PID control para declaration **********************************//
Amber77	0:67bd1a7efc59	14	#define RATE 0.005
Amber77	0:67bd1a7efc59	15	#define Kp_1 1.1 //1.1
Amber77	0:67bd1a7efc59	16	#define Ki_1 0.18 //0.2
Amber77	0:67bd1a7efc59	17	#define Kd_1 0.001
Amber77	0:67bd1a7efc59	18	//************************** Angle Sonsor para declaration **********************************//
Amber77	0:67bd1a7efc59	19	#define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer
Amber77	0:67bd1a7efc59	20	#define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases
Amber77	0:67bd1a7efc59	21	//************************** pragram debug declaration **********************************//
Amber77	0:67bd1a7efc59	22	int index_times = 0;
Amber77	0:67bd1a7efc59	23	double RunTime =0,lastTime =0;
Amber77	0:67bd1a7efc59	24	double t_trajectory=0;
Amber77	0:67bd1a7efc59	25	int do_measure_index=0;
Amber77	0:67bd1a7efc59	26
Amber77	0:67bd1a7efc59	27	double t_MeasureAngularVelocity=0.03;
Amber77	0:67bd1a7efc59	28	double t_PIDcontrol_velocity =0.03;
Amber77	0:67bd1a7efc59	29	double t_LQR_control = 0.05;
Amber77	0:67bd1a7efc59	30	double t_MeasureRobotAttitudeAngle = 0.05;
Amber77	0:67bd1a7efc59	31	double t_quadratureDecoder = 15;
Amber77	0:67bd1a7efc59	32	//double t_TrajectoryTracking = 0.1;
Amber77	0:67bd1a7efc59	33	//************************** Motor para declaration **********************************//
Amber77	0:67bd1a7efc59	34	double r_ball = 0.1; // 球的半徑
Amber77	0:67bd1a7efc59	35	double r_grounder = 0.018; //滾球的半徑
Amber77	0:67bd1a7efc59	36	//************************** Ticker **********************************//
Amber77	0:67bd1a7efc59	37	Serial pc(USBTX, USBRX);
Amber77	0:67bd1a7efc59	38	Ticker Sample_Motor_encoder; // create a timer to sample the encoder.
Amber77	0:67bd1a7efc59	39	Ticker Sample_robotAngleSensor; // create a timer to sample the robot attitude.
Amber77	0:67bd1a7efc59	40	Ticker MeasureAngularVelocity; // create a timer to measure the motor angular velocity.
Amber77	0:67bd1a7efc59	41	Ticker PIDcontrol_velocity; // create a timer to do the motor angular velocity PID control.
Amber77	0:67bd1a7efc59	42	Ticker LQR_control; // create a timer to do the position control.
Amber77	0:67bd1a7efc59	43	//Ticker TrajectoryTracking_control;
Amber77	0:67bd1a7efc59	44	//************************** Measuremant of angular velocity declaration **********************************//
Amber77	0:67bd1a7efc59	45	double Angle_1 =0,Angle_2 =0,Angle_3 =0,Angle_4 =0;
Amber77	0:67bd1a7efc59	46	double LastAngle_1 =0,LastAngle_2 =0,LastAngle_3 =0,LastAngle_4 =0;
Amber77	0:67bd1a7efc59	47	double _1_SectionAngle=0,_1_LastSectionAngle,_1_LastSectionAngle_1,_1_LastSectionAngle_2,_1_LastSectionAngle_3,_1_LastSectionAngle_4;
Amber77	0:67bd1a7efc59	48	double _2_SectionAngle=0,_2_LastSectionAngle,_2_LastSectionAngle_1,_2_LastSectionAngle_2,_2_LastSectionAngle_3,_2_LastSectionAngle_4;
Amber77	0:67bd1a7efc59	49	double _3_SectionAngle=0,_3_LastSectionAngle,_3_LastSectionAngle_1,_3_LastSectionAngle_2,_3_LastSectionAngle_3,_3_LastSectionAngle_4;
Amber77	0:67bd1a7efc59	50	double _4_SectionAngle=0,_4_LastSectionAngle,_4_LastSectionAngle_1,_4_LastSectionAngle_2,_4_LastSectionAngle_3,_4_LastSectionAngle_4;
Amber77	0:67bd1a7efc59	51	double Average_SectionAngle_1,Now_angularVelocity_1=0;
Amber77	0:67bd1a7efc59	52	double Average_SectionAngle_2,Now_angularVelocity_2=0;
Amber77	0:67bd1a7efc59	53	double Average_SectionAngle_3,Now_angularVelocity_3=0;
Amber77	0:67bd1a7efc59	54	double Average_SectionAngle_4,Now_angularVelocity_4=0;
Amber77	0:67bd1a7efc59	55	double Now_angularVelocity_X=0,Now_angularVelocity_Y=0;
Amber77	0:67bd1a7efc59	56	double Angle_X=0, Angle_Y=0;
Amber77	0:67bd1a7efc59	57	double SectionTime =0;
Amber77	0:67bd1a7efc59	58	double NowTime_measureVelocity=0, LastTime_measureVelocity = 0;
Amber77	0:67bd1a7efc59	59	//************************** PID control declaration **********************************//
Amber77	0:67bd1a7efc59	60	float Now_time_PID,Last_Time_PID;
Amber77	0:67bd1a7efc59	61	//Command
Amber77	0:67bd1a7efc59	62	//double Command_AngularVel_1=1,Command_AngularVel_2=1,Command_AngularVel_3=1,Command_AngularVel_4=1;
Amber77	0:67bd1a7efc59	63	//double command_AngularVel_1=1,command_AngularVel_2=1,command_AngularVel_3=1,command_AngularVel_4=1;
Amber77	0:67bd1a7efc59	64	double Command_AngularVel_X=1,Command_AngularVel_Y=1;
Amber77	0:67bd1a7efc59	65	double command_AngularVel_X=1,command_AngularVel_Y=1;
Amber77	0:67bd1a7efc59	66	//Control
Amber77	0:67bd1a7efc59	67	double Control_motor_X,Control_motor_Y;
Amber77	0:67bd1a7efc59	68	//Control PWM value
Amber77	0:67bd1a7efc59	69	double Control_Motor_PWM_X,Control_Motor_PWM_Y;
Amber77	0:67bd1a7efc59	70
Amber77	0:67bd1a7efc59	71
Amber77	0:67bd1a7efc59	72	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);
Amber77	0:67bd1a7efc59	73	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);
Amber77	0:67bd1a7efc59	74	//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);
Amber77	0:67bd1a7efc59	75	//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);
Amber77	0:67bd1a7efc59	76
Amber77	0:67bd1a7efc59	77	//**************************LQR_control declaration**********************************//
Amber77	0:67bd1a7efc59	78	double Ka = 60; //16.9573 25.0
Amber77	0:67bd1a7efc59	79	double Kav = 10; //10.4423 10.7423
Amber77	0:67bd1a7efc59	80
Amber77	0:67bd1a7efc59	81	double Kt = 4.0 ; //3.0 3.1
Amber77	0:67bd1a7efc59	82	double Kt_y = 4.0; //4.0 3.98
Amber77	0:67bd1a7efc59	83
Amber77	0:67bd1a7efc59	84	double Kv = 1.0; //1.0
Amber77	0:67bd1a7efc59	85	double Kv_y = 1.1; //1.1
Amber77	0:67bd1a7efc59	86
Amber77	0:67bd1a7efc59	87	double Kz = 0.05; //0.05
Amber77	0:67bd1a7efc59	88	double Kii = 0.2; //0.2
Amber77	0:67bd1a7efc59	89
Amber77	0:67bd1a7efc59	90	double KI_xy = 0.9; //0.02
Amber77	0:67bd1a7efc59	91	double KI_xy_y = 0.9; //0.025
Amber77	0:67bd1a7efc59	92
Amber77	0:67bd1a7efc59	93	double Roll_offset = -5; //2.88 4.0 3.5
Amber77	0:67bd1a7efc59	94	double Pitch_offset = -4; //-0.05 2.1 2.1
Amber77	0:67bd1a7efc59	95	double Diff_Roll,Diff_Pitch,Diff_Yaw;
Amber77	0:67bd1a7efc59	96	double Integ_Roll,Integ_Pitch;
Amber77	0:67bd1a7efc59	97	double Integ_x,Integ_y;
Amber77	0:67bd1a7efc59	98	double Roll_last,Pitch_last;
Amber77	0:67bd1a7efc59	99	double Vx=0,Vy=0,Wz=0;
Amber77	0:67bd1a7efc59	100	double d_x=0,d_y=0;
Amber77	0:67bd1a7efc59	101	double u_x=0,u_y=0;
Amber77	0:67bd1a7efc59	102	double Point_x;
Amber77	0:67bd1a7efc59	103	double x_now,y_now;
Amber77	0:67bd1a7efc59	104	double x_pre_1,y_pre_1;
Amber77	0:67bd1a7efc59	105	double ax_now,ay_now;
Amber77	0:67bd1a7efc59	106	double ax_pre_1,ay_pre_1;
Amber77	0:67bd1a7efc59	107	double ax_pre_2,ay_pre_2;
Amber77	0:67bd1a7efc59	108	double Vx_pre_1,Vy_pre_1;
Amber77	0:67bd1a7efc59	109	double Diff_x,Diff_y;
Amber77	0:67bd1a7efc59	110	double Diff_x_pre,Diff_y_pre;
Amber77	0:67bd1a7efc59	111	double dot_diff_x,dot_diff_y;
Amber77	0:67bd1a7efc59	112	//**************************Trajectory Tracking declaration**********************************//
Amber77	0:67bd1a7efc59	113	double x_trajectory=0,y_trajectory=0;
Amber77	0:67bd1a7efc59	114	//**************************Encoder declaration**********************************//
Amber77	0:67bd1a7efc59	115	// phase a of the quadrature encoder
Amber77	0:67bd1a7efc59	116	// phase b of the quadrature encoder
Amber77	0:67bd1a7efc59	117	DigitalIn phaseA_1( PA_0 );
Amber77	0:67bd1a7efc59	118	DigitalIn phaseB_1( PA_1 );
Amber77	0:67bd1a7efc59	119	DigitalIn phaseA_2( PA_8 );
Amber77	0:67bd1a7efc59	120	DigitalIn phaseB_2( PA_9 );
Amber77	0:67bd1a7efc59	121	DigitalIn phaseA_3( PC_6 );
Amber77	0:67bd1a7efc59	122	DigitalIn phaseB_3( PC_7 );
Amber77	0:67bd1a7efc59	123	DigitalIn phaseA_4( PB_8 );
Amber77	0:67bd1a7efc59	124	DigitalIn phaseB_4( PB_9 );
Amber77	0:67bd1a7efc59	125	// hold the signed value corresponding to the number of clicks left or right since last sample
Amber77	0:67bd1a7efc59	126	// keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
Amber77	0:67bd1a7efc59	127	int encoderClickCount_1 = 0;
Amber77	0:67bd1a7efc59	128	int previousEncoderState_1 = 0;
Amber77	0:67bd1a7efc59	129	int encoderClickCount_2 = 0;
Amber77	0:67bd1a7efc59	130	int previousEncoderState_2 = 0;
Amber77	0:67bd1a7efc59	131	int encoderClickCount_3 = 0;
Amber77	0:67bd1a7efc59	132	int previousEncoderState_3 = 0;
Amber77	0:67bd1a7efc59	133	int encoderClickCount_4 = 0;
Amber77	0:67bd1a7efc59	134	int previousEncoderState_4 = 0;
Amber77	0:67bd1a7efc59	135	//**************************Angle Sensor declaration**********************************//
Amber77	0:67bd1a7efc59	136	float Times[10] = {0,0,0,0,0,0,0,0,0,0};
Amber77	0:67bd1a7efc59	137	float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz
Amber77	0:67bd1a7efc59	138	int counter = 0;
Amber77	0:67bd1a7efc59	139	int divider = 20;
Amber77	0:67bd1a7efc59	140	float dt; // time for entire loop
Amber77	0:67bd1a7efc59	141	float dt_sensors; // time only to read sensors
Amber77	0:67bd1a7efc59	142	float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
Amber77	0:67bd1a7efc59	143	float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
Amber77	0:67bd1a7efc59	144	float exInt = 0, eyInt = 0, ezInt = 0;
Amber77	0:67bd1a7efc59	145	float OX = 0, OY = 0, OZ = 0;
Amber77	0:67bd1a7efc59	146	float Roll_pre_1,Roll_pre_2,Roll_pre_3,Roll_pre_4;
Amber77	0:67bd1a7efc59	147	float Pitch_pre_1,Pitch_pre_2,Pitch_pre_3,Pitch_pre_4;
Amber77	0:67bd1a7efc59	148	float Mag_x_pre,Mag_y_pre,Mag_z_pre;
Amber77	0:67bd1a7efc59	149	float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
Amber77	0:67bd1a7efc59	150	float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
Amber77	0:67bd1a7efc59	151	float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
Amber77	0:67bd1a7efc59	152	float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
Amber77	0:67bd1a7efc59	153	float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
Amber77	0:67bd1a7efc59	154	float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
Amber77	0:67bd1a7efc59	155	float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
Amber77	0:67bd1a7efc59	156	float Global_mag_vector_angle,Yaw_pre;
Amber77	0:67bd1a7efc59	157	float Global_mag_x_vector_angle,Mag_x_vector_angle;
Amber77	0:67bd1a7efc59	158	float Global_mag_y_vector_angle,Mag_y_vector_angle;
Amber77	0:67bd1a7efc59	159	int Count_mag_check=0;
Amber77	0:67bd1a7efc59	160	float angle[3];
Amber77	0:67bd1a7efc59	161	float Roll,Pitch,Yaw;
Amber77	0:67bd1a7efc59	162	float calibrated_values[3],magCalibrationp[3];
Amber77	0:67bd1a7efc59	163	float v_index[3];
Amber77	0:67bd1a7efc59	164	float dest1,dest2;
Amber77	0:67bd1a7efc59	165	int f=0;
Amber77	0:67bd1a7efc59	166	int j=0;
Amber77	0:67bd1a7efc59	167	int k=0;
Amber77	0:67bd1a7efc59	168	int g=0;
Amber77	0:67bd1a7efc59	169	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;
Amber77	0:67bd1a7efc59	170	int Rot_index;
Amber77	0:67bd1a7efc59	171	float mRes = 10.*4912./32760.0;
Amber77	0:67bd1a7efc59	172
Amber77	0:67bd1a7efc59	173	int AngleFilter_counter = 0;
Amber77	0:67bd1a7efc59	174	float Roll_total = 0,Pitch_total = 0;
Amber77	0:67bd1a7efc59	175	//**************************Motor & SD card & MPU9250 declare**********************************//
Amber77	0:67bd1a7efc59	176	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;
Amber77	0:67bd1a7efc59	177	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;
Amber77	0:67bd1a7efc59	178	int control_LiftingStopRun=1, control_F_R=1, control_H_F=1;
Amber77	0:67bd1a7efc59	179	int Control_LiftingStopRun=1, Control_F_R=1, Control_H_F=1;
Amber77	0:67bd1a7efc59	180	int X_pos=0, Y_pos=0;
Amber77	0:67bd1a7efc59	181	DigitalOut StopRun(PA_12);
Amber77	0:67bd1a7efc59	182	DigitalOut Brake(PA_11);
Amber77	0:67bd1a7efc59	183	DigitalOut AR(PB_12);
Amber77	0:67bd1a7efc59	184	DigitalOut X1_CW_CCW(PC_11);
Amber77	0:67bd1a7efc59	185	DigitalOut X2_CW_CCW(PC_10);
Amber77	0:67bd1a7efc59	186	DigitalOut Y1_CW_CCW(PC_12);
Amber77	0:67bd1a7efc59	187	DigitalOut Y2_CW_CCW(PD_2);
Amber77	0:67bd1a7efc59	188	Servo X_PWM(PB_1);
Amber77	0:67bd1a7efc59	189	Servo Y_PWM(PB_2);
Amber77	0:67bd1a7efc59	190	DigitalOut myled(LED1);
Amber77	0:67bd1a7efc59	191
Amber77	0:67bd1a7efc59	192	//---SD card---//
Amber77	0:67bd1a7efc59	193	SDFileSystem sd( D4, D5, D3, D6, "sd"); // mosi, miso, sclk, cs
Amber77	0:67bd1a7efc59	194	DigitalIn mybutton(USER_BUTTON);
Amber77	0:67bd1a7efc59	195	//---MPU 9250---//
Amber77	0:67bd1a7efc59	196	SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
Amber77	0:67bd1a7efc59	197	//SPI_MOSI = PA_7(D11), SPI_MISO = PA_6(D12), SPI_SCK = PA_5(D13), SPI_CS = PB_6(D10)
Amber77	0:67bd1a7efc59	198	mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object
Amber77	0:67bd1a7efc59	199	//**************************Lifting mechanism & limit switches declare**********************************//
Amber77	0:67bd1a7efc59	200	DigitalOut LiftingStopRun(PA_15);
Amber77	0:67bd1a7efc59	201	DigitalOut F_R(PA_14); // CW/CCW
Amber77	0:67bd1a7efc59	202	DigitalOut H_F(PA_13);
Amber77	0:67bd1a7efc59	203	//DigitalIn TIM(PB_7);
Amber77	0:67bd1a7efc59	204	DigitalIn LimitSwitchUp(PB_15,PullUp);
Amber77	0:67bd1a7efc59	205	DigitalIn LimitSwitchDown(PB_14,PullUp);
Amber77	0:67bd1a7efc59	206	int limitSwitchUp;
Amber77	0:67bd1a7efc59	207	int limitSwitchDown;
Amber77	0:67bd1a7efc59	208	//**************************Timer declaration**********************************//
Amber77	0:67bd1a7efc59	209	Timer NowTime;
Amber77	0:67bd1a7efc59	210	Timer LiftingDownTime;
Amber77	0:67bd1a7efc59	211	//---------------------------------------------------------------------------------//
Amber77	0:67bd1a7efc59	212
Amber77	0:67bd1a7efc59	213	//************************** Filter_IMUupdate **********************************//
Amber77	0:67bd1a7efc59	214	//************************** Filter_IMUupdate **********************************//
Amber77	0:67bd1a7efc59	215	void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az)
Amber77	0:67bd1a7efc59	216	{
Amber77	0:67bd1a7efc59	217	float norm;
Amber77	0:67bd1a7efc59	218	float vx, vy, vz;
Amber77	0:67bd1a7efc59	219	float ex, ey, ez;
Amber77	0:67bd1a7efc59	220
Amber77	0:67bd1a7efc59	221	// normalise the measurements
Amber77	0:67bd1a7efc59	222	norm = sqrt(axax + ayay + az*az);
Amber77	0:67bd1a7efc59	223	if(norm == 0.0f) return;
Amber77	0:67bd1a7efc59	224	ax /= norm;
Amber77	0:67bd1a7efc59	225	ay /= norm;
Amber77	0:67bd1a7efc59	226	az /= norm;
Amber77	0:67bd1a7efc59	227
Amber77	0:67bd1a7efc59	228	// estimated direction of gravity
Amber77	0:67bd1a7efc59	229	vx = 2(q1q3 - q0*q2);
Amber77	0:67bd1a7efc59	230	vy = 2(q0q1 + q2*q3);
Amber77	0:67bd1a7efc59	231	vz = q0q0 - q1q1 - q2q2 + q3q3;
Amber77	0:67bd1a7efc59	232
Amber77	0:67bd1a7efc59	233	// error is sum of cross product between reference direction of field and direction measured by sensor
Amber77	0:67bd1a7efc59	234	ex = (ayvz - azvy);
Amber77	0:67bd1a7efc59	235	ey = (azvx - axvz);
Amber77	0:67bd1a7efc59	236	ez = (axvy - ayvx);
Amber77	0:67bd1a7efc59	237
Amber77	0:67bd1a7efc59	238	// integral error scaled integral gain
Amber77	0:67bd1a7efc59	239	exInt += ex*Ki;
Amber77	0:67bd1a7efc59	240	eyInt += ey*Ki;
Amber77	0:67bd1a7efc59	241	ezInt += ez*Ki;
Amber77	0:67bd1a7efc59	242
Amber77	0:67bd1a7efc59	243	// adjusted gyroscope measurements
Amber77	0:67bd1a7efc59	244	gx += Kp*ex + exInt;
Amber77	0:67bd1a7efc59	245	gy += Kp*ey + eyInt;
Amber77	0:67bd1a7efc59	246	gz += Kp*ez + ezInt;
Amber77	0:67bd1a7efc59	247
Amber77	0:67bd1a7efc59	248	// integrate quaternion rate and normalise
Amber77	0:67bd1a7efc59	249	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!
Amber77	0:67bd1a7efc59	250	float q1o = q1;
Amber77	0:67bd1a7efc59	251	float q2o = q2;
Amber77	0:67bd1a7efc59	252	float q3o = q3;
Amber77	0:67bd1a7efc59	253	q0 += (-q1ogx - q2ogy - q3ogz)halfT;
Amber77	0:67bd1a7efc59	254	q1 += (q0ogx + q2ogz - q3ogy)halfT;
Amber77	0:67bd1a7efc59	255	q2 += (q0ogy - q1ogz + q3ogx)halfT;
Amber77	0:67bd1a7efc59	256	q3 += (q0ogz + q1ogy - q2ogx)halfT;
Amber77	0:67bd1a7efc59	257
Amber77	0:67bd1a7efc59	258	// normalise quaternion
Amber77	0:67bd1a7efc59	259	norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
Amber77	0:67bd1a7efc59	260	q0 = q0 / norm;
Amber77	0:67bd1a7efc59	261	q1 = q1 / norm;
Amber77	0:67bd1a7efc59	262	q2 = q2 / norm;
Amber77	0:67bd1a7efc59	263	q3 = q3 / norm;
Amber77	0:67bd1a7efc59	264	}
Amber77	0:67bd1a7efc59	265
Amber77	0:67bd1a7efc59	266	//************************** Filter_compute **********************************//
Amber77	0:67bd1a7efc59	267	//************************** Filter_compute **********************************//
Amber77	0:67bd1a7efc59	268	void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
Amber77	0:67bd1a7efc59	269	{
Amber77	0:67bd1a7efc59	270	// IMU/AHRS
Amber77	0:67bd1a7efc59	271
Amber77	0:67bd1a7efc59	272	float d_Gyro_angle[3];
Amber77	0:67bd1a7efc59	273	void get_Acc_angle(const float * Acc_data);
Amber77	0:67bd1a7efc59	274	// IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
Amber77	0:67bd1a7efc59	275	float radGyro[3],Gyro_cal_data; // Gyro in radians per second
Amber77	0:67bd1a7efc59	276
Amber77	0:67bd1a7efc59	277	for(int i=0; i<3; i++)
Amber77	0:67bd1a7efc59	278	{
Amber77	0:67bd1a7efc59	279	radGyro[i] = Gyro_data[i] * 3.14159/ 180;
Amber77	0:67bd1a7efc59	280	}
Amber77	0:67bd1a7efc59	281
Amber77	0:67bd1a7efc59	282	Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
Amber77	0:67bd1a7efc59	283	//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]);
Amber77	0:67bd1a7efc59	284
Amber77	0:67bd1a7efc59	285	float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
Amber77	0:67bd1a7efc59	286
Amber77	0:67bd1a7efc59	287	rangle[0] = atan2(2q0q1 + 2q2q3, 1 - 2(q1q1 + q2*q2));
Amber77	0:67bd1a7efc59	288	rangle[1] = asin (2q0q2 - 2q3q1);
Amber77	0:67bd1a7efc59	289	rangle[2] = atan2(2q0_Aq3_A + 2q1_Aq2_A, 1 - 2(q2_Aq2_A + q3_A*q3_A)); // Z-axis
Amber77	0:67bd1a7efc59	290
Amber77	0:67bd1a7efc59	291
Amber77	0:67bd1a7efc59	292	for(int i=0; i<2; i++)
Amber77	0:67bd1a7efc59	293	{ // angle in degree
Amber77	0:67bd1a7efc59	294	angle[i] = rangle[i] * 180 / 3.14159;
Amber77	0:67bd1a7efc59	295	}
Amber77	0:67bd1a7efc59	296	/Roll=angle[0]0.2 + Roll_pre_10.2 + Roll_pre_20.2 +Roll_pre_30.2 + Roll_pre_40.2;
Amber77	0:67bd1a7efc59	297	Roll_pre_4 = Roll_pre_3;
Amber77	0:67bd1a7efc59	298	Roll_pre_3 = Roll_pre_2;
Amber77	0:67bd1a7efc59	299	Roll_pre_2 = Roll_pre_1;
Amber77	0:67bd1a7efc59	300	Roll_pre_1 = Roll;
Amber77	0:67bd1a7efc59	301
Amber77	0:67bd1a7efc59	302	Pitch=angle[1]0.2 + Pitch_pre_10.2 + Pitch_pre_20.2 + Pitch_pre_30.2 + Pitch_pre_4*0.2;
Amber77	0:67bd1a7efc59	303	Pitch_pre_4 = Pitch_pre_3;
Amber77	0:67bd1a7efc59	304	Pitch_pre_3 = Pitch_pre_2;
Amber77	0:67bd1a7efc59	305	Pitch_pre_2 = Pitch_pre_1;
Amber77	0:67bd1a7efc59	306	Pitch_pre_1 = Pitch;*/
Amber77	0:67bd1a7efc59	307
Amber77	0:67bd1a7efc59	308	Roll_total += angle[0];
Amber77	0:67bd1a7efc59	309	Pitch_total += angle[1];
Amber77	0:67bd1a7efc59	310	AngleFilter_counter++;
Amber77	0:67bd1a7efc59	311
Amber77	0:67bd1a7efc59	312	if( AngleFilter_counter > 1 )
Amber77	0:67bd1a7efc59	313	{ // The average of the attitude angle for 100 times.
Amber77	0:67bd1a7efc59	314	Roll = Roll_total / AngleFilter_counter;
Amber77	0:67bd1a7efc59	315	Pitch = Pitch_total / AngleFilter_counter;
Amber77	0:67bd1a7efc59	316	AngleFilter_counter = 0;
Amber77	0:67bd1a7efc59	317	Roll_total = 0;
Amber77	0:67bd1a7efc59	318	Pitch_total = 0;
Amber77	0:67bd1a7efc59	319	Roll -= Roll_offset;
Amber77	0:67bd1a7efc59	320	Pitch -= Pitch_offset;
Amber77	0:67bd1a7efc59	321	}
Amber77	0:67bd1a7efc59	322
Amber77	0:67bd1a7efc59	323	//**************************************************Gyro_data[2] filter start
Amber77	0:67bd1a7efc59	324	float GYRO_z=0;
Amber77	0:67bd1a7efc59	325
Amber77	0:67bd1a7efc59	326	GYRO_z=Gyro_data[2]0.15 + GYRO_z_pre0.20 + GYRO_z_pre_L0.20 + GYRO_z_pre_LL0.25 + GYRO_z_pre_LLL*0.20;
Amber77	0:67bd1a7efc59	327	if( count4==1 )
Amber77	0:67bd1a7efc59	328	{
Amber77	0:67bd1a7efc59	329	GYRO_z_pre_L=GYRO_z_pre;
Amber77	0:67bd1a7efc59	330
Amber77	0:67bd1a7efc59	331	count4=0;
Amber77	0:67bd1a7efc59	332	}
Amber77	0:67bd1a7efc59	333	if( count5==2 )
Amber77	0:67bd1a7efc59	334	{
Amber77	0:67bd1a7efc59	335	GYRO_z_pre_LL=GYRO_z_pre_L;
Amber77	0:67bd1a7efc59	336
Amber77	0:67bd1a7efc59	337	count5=0;
Amber77	0:67bd1a7efc59	338	}
Amber77	0:67bd1a7efc59	339	if( count6==3 )
Amber77	0:67bd1a7efc59	340	{
Amber77	0:67bd1a7efc59	341	GYRO_z_pre_LLL=GYRO_z_pre_LL;
Amber77	0:67bd1a7efc59	342
Amber77	0:67bd1a7efc59	343	count6=0;
Amber77	0:67bd1a7efc59	344	}
Amber77	0:67bd1a7efc59	345	count4++;
Amber77	0:67bd1a7efc59	346	count5++;
Amber77	0:67bd1a7efc59	347	count6++;
Amber77	0:67bd1a7efc59	348	GYRO_z_pre=Gyro_data[2];
Amber77	0:67bd1a7efc59	349	Global_GYRO_z=GYRO_z;
Amber77	0:67bd1a7efc59	350	/*printf(" GYRO_z:%10.3f ,count8:%10d \n",
Amber77	0:67bd1a7efc59	351	GYRO_z,
Amber77	0:67bd1a7efc59	352	count8
Amber77	0:67bd1a7efc59	353	);*/
Amber77	0:67bd1a7efc59	354	if((count8>5)&&(count8<=2005))
Amber77	0:67bd1a7efc59	355	{
Amber77	0:67bd1a7efc59	356	GYRO_z_total+=GYRO_z;
Amber77	0:67bd1a7efc59	357	}
Amber77	0:67bd1a7efc59	358	if( count8==2005 )
Amber77	0:67bd1a7efc59	359	{
Amber77	0:67bd1a7efc59	360	GYRO_z_offset=GYRO_z_total/2000;
Amber77	0:67bd1a7efc59	361	/* printf(" GYRO_z_offset:%10.5f \n ",
Amber77	0:67bd1a7efc59	362	GYRO_z_offset
Amber77	0:67bd1a7efc59	363	);*/
Amber77	0:67bd1a7efc59	364	GYRO_z_total=0;
Amber77	0:67bd1a7efc59	365	count8=0;
Amber77	0:67bd1a7efc59	366	}
Amber77	0:67bd1a7efc59	367
Amber77	0:67bd1a7efc59	368	count8++;
Amber77	0:67bd1a7efc59	369	//**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 0.74956
Amber77	0:67bd1a7efc59	370	//************************************************** calculate Yaw
Amber77	0:67bd1a7efc59	371	if( (count11==35) )
Amber77	0:67bd1a7efc59	372	{
Amber77	0:67bd1a7efc59	373	if( abs(Yaw_pre-Yaw)<1 )
Amber77	0:67bd1a7efc59	374	{
Amber77	0:67bd1a7efc59	375	Yaw_pre=Yaw_pre;
Amber77	0:67bd1a7efc59	376	}
Amber77	0:67bd1a7efc59	377	else
Amber77	0:67bd1a7efc59	378	{
Amber77	0:67bd1a7efc59	379	Yaw_pre=Yaw;
Amber77	0:67bd1a7efc59	380	}
Amber77	0:67bd1a7efc59	381	count11=0;
Amber77	0:67bd1a7efc59	382	}
Amber77	0:67bd1a7efc59	383	count11++;
Amber77	0:67bd1a7efc59	384
Amber77	0:67bd1a7efc59	385	if( count12>=20 )
Amber77	0:67bd1a7efc59	386	{
Amber77	0:67bd1a7efc59	387	Yaw += (Gyro_data[2]-0.74936) *dt;
Amber77	0:67bd1a7efc59	388	}
Amber77	0:67bd1a7efc59	389	count12++;
Amber77	0:67bd1a7efc59	390	//pc.printf(" Yaw:%10.5f ",
Amber77	0:67bd1a7efc59	391	//Yaw
Amber77	0:67bd1a7efc59	392	// );
Amber77	0:67bd1a7efc59	393	}
Amber77	0:67bd1a7efc59	394
Amber77	0:67bd1a7efc59	395	//************************** Mag_Complentary_Filter **********************************//
Amber77	0:67bd1a7efc59	396	void Mag_Complentary_Filter(float dt, const float * Comp_data)
Amber77	0:67bd1a7efc59	397	{
Amber77	0:67bd1a7efc59	398	float Mag_x=0,Mag_y=0,Mag_z=0;
Amber77	0:67bd1a7efc59	399	Mag_x=Comp_data[0]0.15 + Mag_x_pre0.20 + Mag_x_pre_L0.20 + Mag_x_pre_LL0.25 + Mag_x_pre_LLL*0.20;
Amber77	0:67bd1a7efc59	400	Mag_y=Comp_data[1]0.15 + Mag_y_pre0.20 + Mag_y_pre_L0.20 + Mag_y_pre_LL0.25 + Mag_y_pre_LLL*0.20;
Amber77	0:67bd1a7efc59	401	Mag_z=Comp_data[2]0.15 + Mag_z_pre0.20 + Mag_z_pre_L0.20 + Mag_z_pre_LL0.25 + Mag_z_pre_LLL*0.20;
Amber77	0:67bd1a7efc59	402
Amber77	0:67bd1a7efc59	403	if( count1==1 )
Amber77	0:67bd1a7efc59	404	{
Amber77	0:67bd1a7efc59	405	Mag_x_pre_L=Mag_x_pre;
Amber77	0:67bd1a7efc59	406	Mag_y_pre_L=Mag_y_pre;
Amber77	0:67bd1a7efc59	407	Mag_z_pre_L=Mag_z_pre;
Amber77	0:67bd1a7efc59	408	Cal_Mag_x_pre=Cal_Mag_x;
Amber77	0:67bd1a7efc59	409
Amber77	0:67bd1a7efc59	410	count1=0;
Amber77	0:67bd1a7efc59	411	}
Amber77	0:67bd1a7efc59	412	if( count2==2 )
Amber77	0:67bd1a7efc59	413	{
Amber77	0:67bd1a7efc59	414	Mag_x_pre_LL=Mag_x_pre_L;
Amber77	0:67bd1a7efc59	415	Mag_y_pre_LL=Mag_y_pre_L;
Amber77	0:67bd1a7efc59	416	Mag_z_pre_LL=Mag_z_pre_L;
Amber77	0:67bd1a7efc59	417	Cal_Mag_x_pre_L=Cal_Mag_x_pre;
Amber77	0:67bd1a7efc59	418
Amber77	0:67bd1a7efc59	419	count2=0;
Amber77	0:67bd1a7efc59	420	}
Amber77	0:67bd1a7efc59	421	if( count7==3 )
Amber77	0:67bd1a7efc59	422	{
Amber77	0:67bd1a7efc59	423	Mag_x_pre_LLL=Mag_x_pre_LL;
Amber77	0:67bd1a7efc59	424	Mag_y_pre_LLL=Mag_y_pre_LL;
Amber77	0:67bd1a7efc59	425	Mag_z_pre_LLL=Mag_z_pre_LL;
Amber77	0:67bd1a7efc59	426	Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
Amber77	0:67bd1a7efc59	427
Amber77	0:67bd1a7efc59	428	count7=0;
Amber77	0:67bd1a7efc59	429	}
Amber77	0:67bd1a7efc59	430	count1++;
Amber77	0:67bd1a7efc59	431	count2++;
Amber77	0:67bd1a7efc59	432	count7++;
Amber77	0:67bd1a7efc59	433	Mag_x_pre=Comp_data[0];
Amber77	0:67bd1a7efc59	434	Mag_y_pre=Comp_data[1];
Amber77	0:67bd1a7efc59	435	Mag_z_pre=Comp_data[2];
Amber77	0:67bd1a7efc59	436	if( count14>4 )
Amber77	0:67bd1a7efc59	437	{
Amber77	0:67bd1a7efc59	438	Cal_Mag_x=Mag_x;
Amber77	0:67bd1a7efc59	439	}
Amber77	0:67bd1a7efc59	440	count14++;
Amber77	0:67bd1a7efc59	441
Amber77	0:67bd1a7efc59	442
Amber77	0:67bd1a7efc59	443	//*************************************Mag_ave calculate
Amber77	0:67bd1a7efc59	444	if(count3<=20)
Amber77	0:67bd1a7efc59	445	{
Amber77	0:67bd1a7efc59	446	Mag_x_total+=Mag_x;
Amber77	0:67bd1a7efc59	447	Mag_y_total+=Mag_y;
Amber77	0:67bd1a7efc59	448	}
Amber77	0:67bd1a7efc59	449	if( count3==20)
Amber77	0:67bd1a7efc59	450	{
Amber77	0:67bd1a7efc59	451	Mag_x_ave=Mag_x_total/21;
Amber77	0:67bd1a7efc59	452	Mag_y_ave=Mag_y_total/21;
Amber77	0:67bd1a7efc59	453	/*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ",
Amber77	0:67bd1a7efc59	454	Mag_x_ave,
Amber77	0:67bd1a7efc59	455	Mag_y_ave
Amber77	0:67bd1a7efc59	456	);*/
Amber77	0:67bd1a7efc59	457	Mag_x_total=0;
Amber77	0:67bd1a7efc59	458	Mag_y_total=0;
Amber77	0:67bd1a7efc59	459	count3=0;
Amber77	0:67bd1a7efc59	460	}
Amber77	0:67bd1a7efc59	461	count3++;
Amber77	0:67bd1a7efc59	462
Amber77	0:67bd1a7efc59	463	//********************************ROT_check start
Amber77	0:67bd1a7efc59	464
Amber77	0:67bd1a7efc59	465	float v_length,v_length_ave,MagVector_angle;
Amber77	0:67bd1a7efc59	466	v_length=sqrt( Mag_xMag_x + Mag_yMag_y );
Amber77	0:67bd1a7efc59	467	v_length_ave=sqrt( Mag_x_aveMag_x_ave + Mag_y_aveMag_y_ave );
Amber77	0:67bd1a7efc59	468
Amber77	0:67bd1a7efc59	469	MagVector_angle=acos(( Mag_xMag_x_ave + Mag_yMag_y_ave )/(v_lengthv_length_ave))57.3;
Amber77	0:67bd1a7efc59	470
Amber77	0:67bd1a7efc59	471	if( count9==3 )
Amber77	0:67bd1a7efc59	472	{
Amber77	0:67bd1a7efc59	473	Global_mag_vector_angle=MagVector_angle;
Amber77	0:67bd1a7efc59	474	count9=0;
Amber77	0:67bd1a7efc59	475	}
Amber77	0:67bd1a7efc59	476	count9++;
Amber77	0:67bd1a7efc59	477
Amber77	0:67bd1a7efc59	478	if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) )
Amber77	0:67bd1a7efc59	479	{
Amber77	0:67bd1a7efc59	480	Count_mag_check++;
Amber77	0:67bd1a7efc59	481
Amber77	0:67bd1a7efc59	482	}
Amber77	0:67bd1a7efc59	483	else
Amber77	0:67bd1a7efc59	484	{
Amber77	0:67bd1a7efc59	485	Count_mag_check=0;
Amber77	0:67bd1a7efc59	486	}
Amber77	0:67bd1a7efc59	487
Amber77	0:67bd1a7efc59	488	if( Count_mag_check==30 )
Amber77	0:67bd1a7efc59	489	{
Amber77	0:67bd1a7efc59	490	Yaw=Yaw_pre;
Amber77	0:67bd1a7efc59	491	Count_mag_check=0;
Amber77	0:67bd1a7efc59	492	}
Amber77	0:67bd1a7efc59	493	float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
Amber77	0:67bd1a7efc59	494	//********************************Theta_check end
Amber77	0:67bd1a7efc59	495	/*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 ",
Amber77	0:67bd1a7efc59	496	ABS_CHECK,
Amber77	0:67bd1a7efc59	497	Cal_Mag_x_pre_LL,
Amber77	0:67bd1a7efc59	498	Mag_x,
Amber77	0:67bd1a7efc59	499	Count_mag_check,
Amber77	0:67bd1a7efc59	500	Yaw_pre,
Amber77	0:67bd1a7efc59	501	Yaw
Amber77	0:67bd1a7efc59	502	);*/
Amber77	0:67bd1a7efc59	503	}
Amber77	0:67bd1a7efc59	504	//**************************Motor_run**********************************//
Amber77	0:67bd1a7efc59	505	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)
Amber77	0:67bd1a7efc59	506	{
Amber77	0:67bd1a7efc59	507	StopRun.write(control_stopRun);
Amber77	0:67bd1a7efc59	508	Brake.write(control_brake);
Amber77	0:67bd1a7efc59	509	AR.write(control_AR);
Amber77	0:67bd1a7efc59	510	X1_CW_CCW.write(control_X1_CW_CCW);
Amber77	0:67bd1a7efc59	511	X2_CW_CCW.write(control_X2_CW_CCW);
Amber77	0:67bd1a7efc59	512	Y1_CW_CCW.write(control_Y1_CW_CCW);
Amber77	0:67bd1a7efc59	513	Y2_CW_CCW.write(control_Y2_CW_CCW);
Amber77	0:67bd1a7efc59	514
Amber77	0:67bd1a7efc59	515
Amber77	0:67bd1a7efc59	516	X_PWM.write(control_value_PWM_X);
Amber77	0:67bd1a7efc59	517	Y_PWM.write(control_value_PWM_Y);
Amber77	0:67bd1a7efc59	518	}
Amber77	0:67bd1a7efc59	519
Amber77	0:67bd1a7efc59	520	//**************************quadratureDecoder**********************************//
Amber77	0:67bd1a7efc59	521	void quadratureDecoder( void )
Amber77	0:67bd1a7efc59	522	{
Amber77	0:67bd1a7efc59	523	int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read();
Amber77	0:67bd1a7efc59	524	int currentEncoderState_2 = (phaseB_2.read() << 1) + phaseA_2.read();
Amber77	0:67bd1a7efc59	525	int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read();
Amber77	0:67bd1a7efc59	526	int currentEncoderState_4 = (phaseB_4.read() << 1) + phaseA_4.read();
Amber77	0:67bd1a7efc59	527	//************************** 1 **********************************//
Amber77	0:67bd1a7efc59	528	if( currentEncoderState_1 == previousEncoderState_1 )
Amber77	0:67bd1a7efc59	529	{
Amber77	0:67bd1a7efc59	530	return;
Amber77	0:67bd1a7efc59	531	}
Amber77	0:67bd1a7efc59	532
Amber77	0:67bd1a7efc59	533	switch( previousEncoderState_1 )
Amber77	0:67bd1a7efc59	534	{
Amber77	0:67bd1a7efc59	535	case 0:
Amber77	0:67bd1a7efc59	536	if( currentEncoderState_1 == 2 )
Amber77	0:67bd1a7efc59	537	{
Amber77	0:67bd1a7efc59	538	encoderClickCount_1--;
Amber77	0:67bd1a7efc59	539
Amber77	0:67bd1a7efc59	540	}
Amber77	0:67bd1a7efc59	541	else if( currentEncoderState_1 == 1 )
Amber77	0:67bd1a7efc59	542	{
Amber77	0:67bd1a7efc59	543	encoderClickCount_1++;
Amber77	0:67bd1a7efc59	544
Amber77	0:67bd1a7efc59	545	}
Amber77	0:67bd1a7efc59	546	break;
Amber77	0:67bd1a7efc59	547
Amber77	0:67bd1a7efc59	548	case 1:
Amber77	0:67bd1a7efc59	549	if( currentEncoderState_1 == 0 )
Amber77	0:67bd1a7efc59	550	{
Amber77	0:67bd1a7efc59	551	encoderClickCount_1--;
Amber77	0:67bd1a7efc59	552
Amber77	0:67bd1a7efc59	553	}
Amber77	0:67bd1a7efc59	554	else if( currentEncoderState_1 == 3 )
Amber77	0:67bd1a7efc59	555	{
Amber77	0:67bd1a7efc59	556	encoderClickCount_1++;
Amber77	0:67bd1a7efc59	557
Amber77	0:67bd1a7efc59	558	}
Amber77	0:67bd1a7efc59	559	break;
Amber77	0:67bd1a7efc59	560
Amber77	0:67bd1a7efc59	561	case 2:
Amber77	0:67bd1a7efc59	562	if( currentEncoderState_1 == 3 )
Amber77	0:67bd1a7efc59	563	{
Amber77	0:67bd1a7efc59	564	encoderClickCount_1--;
Amber77	0:67bd1a7efc59	565
Amber77	0:67bd1a7efc59	566	}
Amber77	0:67bd1a7efc59	567	else if( currentEncoderState_1 == 0 )
Amber77	0:67bd1a7efc59	568	{
Amber77	0:67bd1a7efc59	569	encoderClickCount_1++;
Amber77	0:67bd1a7efc59	570
Amber77	0:67bd1a7efc59	571	}
Amber77	0:67bd1a7efc59	572	break;
Amber77	0:67bd1a7efc59	573
Amber77	0:67bd1a7efc59	574	case 3:
Amber77	0:67bd1a7efc59	575	if( currentEncoderState_1 == 1 )
Amber77	0:67bd1a7efc59	576	{
Amber77	0:67bd1a7efc59	577	encoderClickCount_1--;
Amber77	0:67bd1a7efc59	578
Amber77	0:67bd1a7efc59	579	}
Amber77	0:67bd1a7efc59	580	else if( currentEncoderState_1 == 2 )
Amber77	0:67bd1a7efc59	581	{
Amber77	0:67bd1a7efc59	582	encoderClickCount_1++;
Amber77	0:67bd1a7efc59	583
Amber77	0:67bd1a7efc59	584	}
Amber77	0:67bd1a7efc59	585	break;
Amber77	0:67bd1a7efc59	586
Amber77	0:67bd1a7efc59	587	default:
Amber77	0:67bd1a7efc59	588	break;
Amber77	0:67bd1a7efc59	589	}
Amber77	0:67bd1a7efc59	590	previousEncoderState_1 = currentEncoderState_1;
Amber77	0:67bd1a7efc59	591	//************************** 2 **********************************//
Amber77	0:67bd1a7efc59	592	if( currentEncoderState_2 == previousEncoderState_2 )
Amber77	0:67bd1a7efc59	593	{
Amber77	0:67bd1a7efc59	594	return;
Amber77	0:67bd1a7efc59	595	}
Amber77	0:67bd1a7efc59	596
Amber77	0:67bd1a7efc59	597	switch( previousEncoderState_2 )
Amber77	0:67bd1a7efc59	598	{
Amber77	0:67bd1a7efc59	599	case 0:
Amber77	0:67bd1a7efc59	600	if( currentEncoderState_2 == 2 )
Amber77	0:67bd1a7efc59	601	{
Amber77	0:67bd1a7efc59	602	encoderClickCount_2--;
Amber77	0:67bd1a7efc59	603
Amber77	0:67bd1a7efc59	604	}
Amber77	0:67bd1a7efc59	605	else if( currentEncoderState_2 == 1 )
Amber77	0:67bd1a7efc59	606	{
Amber77	0:67bd1a7efc59	607	encoderClickCount_2++;
Amber77	0:67bd1a7efc59	608
Amber77	0:67bd1a7efc59	609	}
Amber77	0:67bd1a7efc59	610	break;
Amber77	0:67bd1a7efc59	611
Amber77	0:67bd1a7efc59	612	case 1:
Amber77	0:67bd1a7efc59	613	if( currentEncoderState_2 == 0 )
Amber77	0:67bd1a7efc59	614	{
Amber77	0:67bd1a7efc59	615	encoderClickCount_2--;
Amber77	0:67bd1a7efc59	616
Amber77	0:67bd1a7efc59	617	}
Amber77	0:67bd1a7efc59	618	else if( currentEncoderState_2 == 3 )
Amber77	0:67bd1a7efc59	619	{
Amber77	0:67bd1a7efc59	620	encoderClickCount_2++;
Amber77	0:67bd1a7efc59	621
Amber77	0:67bd1a7efc59	622	}
Amber77	0:67bd1a7efc59	623	break;
Amber77	0:67bd1a7efc59	624
Amber77	0:67bd1a7efc59	625	case 2:
Amber77	0:67bd1a7efc59	626	if( currentEncoderState_2 == 3 )
Amber77	0:67bd1a7efc59	627	{
Amber77	0:67bd1a7efc59	628	encoderClickCount_2--;
Amber77	0:67bd1a7efc59	629
Amber77	0:67bd1a7efc59	630	}
Amber77	0:67bd1a7efc59	631	else if( currentEncoderState_2 == 0 )
Amber77	0:67bd1a7efc59	632	{
Amber77	0:67bd1a7efc59	633	encoderClickCount_2++;
Amber77	0:67bd1a7efc59	634
Amber77	0:67bd1a7efc59	635	}
Amber77	0:67bd1a7efc59	636	break;
Amber77	0:67bd1a7efc59	637
Amber77	0:67bd1a7efc59	638	case 3:
Amber77	0:67bd1a7efc59	639	if( currentEncoderState_2 == 1 )
Amber77	0:67bd1a7efc59	640	{
Amber77	0:67bd1a7efc59	641	encoderClickCount_2--;
Amber77	0:67bd1a7efc59	642
Amber77	0:67bd1a7efc59	643	}
Amber77	0:67bd1a7efc59	644	else if( currentEncoderState_2 == 2 )
Amber77	0:67bd1a7efc59	645	{
Amber77	0:67bd1a7efc59	646	encoderClickCount_2++;
Amber77	0:67bd1a7efc59	647
Amber77	0:67bd1a7efc59	648	}
Amber77	0:67bd1a7efc59	649	break;
Amber77	0:67bd1a7efc59	650
Amber77	0:67bd1a7efc59	651	default:
Amber77	0:67bd1a7efc59	652	break;
Amber77	0:67bd1a7efc59	653	}
Amber77	0:67bd1a7efc59	654	previousEncoderState_2 = currentEncoderState_2;
Amber77	0:67bd1a7efc59	655	//************************** 3 **********************************//
Amber77	0:67bd1a7efc59	656	if( currentEncoderState_3 == previousEncoderState_3 )
Amber77	0:67bd1a7efc59	657	{
Amber77	0:67bd1a7efc59	658	return;
Amber77	0:67bd1a7efc59	659	}
Amber77	0:67bd1a7efc59	660
Amber77	0:67bd1a7efc59	661	switch( previousEncoderState_3 )
Amber77	0:67bd1a7efc59	662	{
Amber77	0:67bd1a7efc59	663	case 0:
Amber77	0:67bd1a7efc59	664	if( currentEncoderState_3 == 2 )
Amber77	0:67bd1a7efc59	665	{
Amber77	0:67bd1a7efc59	666	encoderClickCount_3--;
Amber77	0:67bd1a7efc59	667
Amber77	0:67bd1a7efc59	668	}
Amber77	0:67bd1a7efc59	669	else if( currentEncoderState_3 == 1 )
Amber77	0:67bd1a7efc59	670	{
Amber77	0:67bd1a7efc59	671	encoderClickCount_3++;
Amber77	0:67bd1a7efc59	672
Amber77	0:67bd1a7efc59	673	}
Amber77	0:67bd1a7efc59	674	break;
Amber77	0:67bd1a7efc59	675
Amber77	0:67bd1a7efc59	676	case 1:
Amber77	0:67bd1a7efc59	677	if( currentEncoderState_3 == 0 )
Amber77	0:67bd1a7efc59	678	{
Amber77	0:67bd1a7efc59	679	encoderClickCount_3--;
Amber77	0:67bd1a7efc59	680
Amber77	0:67bd1a7efc59	681	}
Amber77	0:67bd1a7efc59	682	else if( currentEncoderState_3 == 3 )
Amber77	0:67bd1a7efc59	683	{
Amber77	0:67bd1a7efc59	684	encoderClickCount_3++;
Amber77	0:67bd1a7efc59	685
Amber77	0:67bd1a7efc59	686	}
Amber77	0:67bd1a7efc59	687	break;
Amber77	0:67bd1a7efc59	688
Amber77	0:67bd1a7efc59	689	case 2:
Amber77	0:67bd1a7efc59	690	if( currentEncoderState_3 == 3 )
Amber77	0:67bd1a7efc59	691	{
Amber77	0:67bd1a7efc59	692	encoderClickCount_3--;
Amber77	0:67bd1a7efc59	693
Amber77	0:67bd1a7efc59	694	}
Amber77	0:67bd1a7efc59	695	else if( currentEncoderState_3 == 0 )
Amber77	0:67bd1a7efc59	696	{
Amber77	0:67bd1a7efc59	697	encoderClickCount_3++;
Amber77	0:67bd1a7efc59	698
Amber77	0:67bd1a7efc59	699	}
Amber77	0:67bd1a7efc59	700	break;
Amber77	0:67bd1a7efc59	701
Amber77	0:67bd1a7efc59	702	case 3:
Amber77	0:67bd1a7efc59	703	if( currentEncoderState_3 == 1 )
Amber77	0:67bd1a7efc59	704	{
Amber77	0:67bd1a7efc59	705	encoderClickCount_3--;
Amber77	0:67bd1a7efc59	706
Amber77	0:67bd1a7efc59	707	}
Amber77	0:67bd1a7efc59	708	else if( currentEncoderState_3 == 2 )
Amber77	0:67bd1a7efc59	709	{
Amber77	0:67bd1a7efc59	710	encoderClickCount_3++;
Amber77	0:67bd1a7efc59	711
Amber77	0:67bd1a7efc59	712	}
Amber77	0:67bd1a7efc59	713	break;
Amber77	0:67bd1a7efc59	714
Amber77	0:67bd1a7efc59	715	default:
Amber77	0:67bd1a7efc59	716	break;
Amber77	0:67bd1a7efc59	717	}
Amber77	0:67bd1a7efc59	718	previousEncoderState_3 = currentEncoderState_3;
Amber77	0:67bd1a7efc59	719	//************************** 4 **********************************//
Amber77	0:67bd1a7efc59	720	if( currentEncoderState_4 == previousEncoderState_4 )
Amber77	0:67bd1a7efc59	721	{
Amber77	0:67bd1a7efc59	722	return;
Amber77	0:67bd1a7efc59	723	}
Amber77	0:67bd1a7efc59	724
Amber77	0:67bd1a7efc59	725	switch( previousEncoderState_4 )
Amber77	0:67bd1a7efc59	726	{
Amber77	0:67bd1a7efc59	727	case 0:
Amber77	0:67bd1a7efc59	728	if( currentEncoderState_4 == 2 )
Amber77	0:67bd1a7efc59	729	{
Amber77	0:67bd1a7efc59	730	encoderClickCount_4--;
Amber77	0:67bd1a7efc59	731
Amber77	0:67bd1a7efc59	732	}
Amber77	0:67bd1a7efc59	733	else if( currentEncoderState_4 == 1 )
Amber77	0:67bd1a7efc59	734	{
Amber77	0:67bd1a7efc59	735	encoderClickCount_4++;
Amber77	0:67bd1a7efc59	736
Amber77	0:67bd1a7efc59	737	}
Amber77	0:67bd1a7efc59	738	break;
Amber77	0:67bd1a7efc59	739
Amber77	0:67bd1a7efc59	740	case 1:
Amber77	0:67bd1a7efc59	741	if( currentEncoderState_4 == 0 )
Amber77	0:67bd1a7efc59	742	{
Amber77	0:67bd1a7efc59	743	encoderClickCount_4--;
Amber77	0:67bd1a7efc59	744
Amber77	0:67bd1a7efc59	745	}
Amber77	0:67bd1a7efc59	746	else if( currentEncoderState_4 == 3 )
Amber77	0:67bd1a7efc59	747	{
Amber77	0:67bd1a7efc59	748	encoderClickCount_4++;
Amber77	0:67bd1a7efc59	749
Amber77	0:67bd1a7efc59	750	}
Amber77	0:67bd1a7efc59	751	break;
Amber77	0:67bd1a7efc59	752
Amber77	0:67bd1a7efc59	753	case 2:
Amber77	0:67bd1a7efc59	754	if( currentEncoderState_4 == 3 )
Amber77	0:67bd1a7efc59	755	{
Amber77	0:67bd1a7efc59	756	encoderClickCount_4--;
Amber77	0:67bd1a7efc59	757
Amber77	0:67bd1a7efc59	758	}
Amber77	0:67bd1a7efc59	759	else if( currentEncoderState_4 == 0 )
Amber77	0:67bd1a7efc59	760	{
Amber77	0:67bd1a7efc59	761	encoderClickCount_4++;
Amber77	0:67bd1a7efc59	762
Amber77	0:67bd1a7efc59	763	}
Amber77	0:67bd1a7efc59	764	break;
Amber77	0:67bd1a7efc59	765
Amber77	0:67bd1a7efc59	766	case 3:
Amber77	0:67bd1a7efc59	767	if( currentEncoderState_4 == 1 )
Amber77	0:67bd1a7efc59	768	{
Amber77	0:67bd1a7efc59	769	encoderClickCount_4--;
Amber77	0:67bd1a7efc59	770
Amber77	0:67bd1a7efc59	771	}
Amber77	0:67bd1a7efc59	772	else if( currentEncoderState_4 == 2 )
Amber77	0:67bd1a7efc59	773	{
Amber77	0:67bd1a7efc59	774	encoderClickCount_4++;
Amber77	0:67bd1a7efc59	775
Amber77	0:67bd1a7efc59	776	}
Amber77	0:67bd1a7efc59	777	break;
Amber77	0:67bd1a7efc59	778
Amber77	0:67bd1a7efc59	779	default:
Amber77	0:67bd1a7efc59	780	break;
Amber77	0:67bd1a7efc59	781	}
Amber77	0:67bd1a7efc59	782	previousEncoderState_4 = currentEncoderState_4;
Amber77	0:67bd1a7efc59	783	}
Amber77	0:67bd1a7efc59	784
Amber77	0:67bd1a7efc59	785	//**************************getAngular**********************************//
Amber77	0:67bd1a7efc59	786	//**************************getAngular**********************************//
Amber77	0:67bd1a7efc59	787	void getAngular( void )
Amber77	0:67bd1a7efc59	788	{
Amber77	0:67bd1a7efc59	789	// Angle_1 = (encoderClickCount_1*0.1499)/5;
Amber77	0:67bd1a7efc59	790	// Angle_2 = (encoderClickCount_2*0.1499)/5;
Amber77	0:67bd1a7efc59	791	// Angle_3 = (encoderClickCount_3*0.1499)/5;
Amber77	0:67bd1a7efc59	792	// Angle_4 = (encoderClickCount_4*0.1499)/5;
Amber77	0:67bd1a7efc59	793	Angle_1 = encoderClickCount_1*0.03;
Amber77	0:67bd1a7efc59	794	Angle_2 = encoderClickCount_2*0.03;
Amber77	0:67bd1a7efc59	795	Angle_3 = encoderClickCount_3*0.03;
Amber77	0:67bd1a7efc59	796	Angle_4 = encoderClickCount_4*0.03;
Amber77	0:67bd1a7efc59	797
Amber77	0:67bd1a7efc59	798	_1_SectionAngle = Angle_1 - LastAngle_1;
Amber77	0:67bd1a7efc59	799	_2_SectionAngle = Angle_2 - LastAngle_2;
Amber77	0:67bd1a7efc59	800	_3_SectionAngle = Angle_3 - LastAngle_3;
Amber77	0:67bd1a7efc59	801	_4_SectionAngle = Angle_4 - LastAngle_4;
Amber77	0:67bd1a7efc59	802	Average_SectionAngle_1 = (_1_SectionAngle0.3 + _1_LastSectionAngle0.3 + _1_LastSectionAngle_10.1 + _1_LastSectionAngle_20.1 + _1_LastSectionAngle_30.1 + _1_LastSectionAngle_40.1)/1;
Amber77	0:67bd1a7efc59	803	Average_SectionAngle_2 = (_2_SectionAngle0.3 + _2_LastSectionAngle0.3 + _2_LastSectionAngle_10.1 + _2_LastSectionAngle_20.1 + _2_LastSectionAngle_30.1 + _2_LastSectionAngle_40.1)/1;
Amber77	0:67bd1a7efc59	804	Average_SectionAngle_3 = (_3_SectionAngle0.3 + _3_LastSectionAngle0.3 + _3_LastSectionAngle_10.1 + _3_LastSectionAngle_20.1 + _3_LastSectionAngle_30.1 + _3_LastSectionAngle_40.1)/1;
Amber77	0:67bd1a7efc59	805	Average_SectionAngle_4 = (_4_SectionAngle0.3 + _4_LastSectionAngle0.3 + _4_LastSectionAngle_10.1 + _4_LastSectionAngle_20.1 + _4_LastSectionAngle_30.1 + _4_LastSectionAngle_40.1)/1;
Amber77	0:67bd1a7efc59	806	NowTime_measureVelocity = NowTime.read();
Amber77	0:67bd1a7efc59	807	SectionTime = NowTime_measureVelocity - LastTime_measureVelocity;
Amber77	0:67bd1a7efc59	808	Now_angularVelocity_1 = abs(Average_SectionAngle_1/(SectionTime));
Amber77	0:67bd1a7efc59	809	Now_angularVelocity_2 = abs(Average_SectionAngle_2/(SectionTime));
Amber77	0:67bd1a7efc59	810	Now_angularVelocity_3 = abs(Average_SectionAngle_3/(SectionTime));
Amber77	0:67bd1a7efc59	811	Now_angularVelocity_4 = abs(Average_SectionAngle_4/(SectionTime));
Amber77	0:67bd1a7efc59	812	Now_angularVelocity_X = (Now_angularVelocity_2+Now_angularVelocity_4)*0.5;
Amber77	0:67bd1a7efc59	813	Now_angularVelocity_Y = (Now_angularVelocity_1+Now_angularVelocity_3)*0.5;
Amber77	0:67bd1a7efc59	814
Amber77	0:67bd1a7efc59	815
Amber77	0:67bd1a7efc59	816	LastTime_measureVelocity = NowTime_measureVelocity;
Amber77	0:67bd1a7efc59	817	LastAngle_1 = Angle_1;
Amber77	0:67bd1a7efc59	818	_1_LastSectionAngle_4 = _1_LastSectionAngle_3;
Amber77	0:67bd1a7efc59	819	_1_LastSectionAngle_3 = _1_LastSectionAngle_2;
Amber77	0:67bd1a7efc59	820	_1_LastSectionAngle_2 = _1_LastSectionAngle_1;
Amber77	0:67bd1a7efc59	821	_1_LastSectionAngle_1 = _1_LastSectionAngle;
Amber77	0:67bd1a7efc59	822	_1_LastSectionAngle = _1_SectionAngle;
Amber77	0:67bd1a7efc59	823	LastAngle_2 = Angle_2;
Amber77	0:67bd1a7efc59	824	_2_LastSectionAngle_4 = _2_LastSectionAngle_3;
Amber77	0:67bd1a7efc59	825	_2_LastSectionAngle_3 = _2_LastSectionAngle_2;
Amber77	0:67bd1a7efc59	826	_2_LastSectionAngle_2 = _2_LastSectionAngle_1;
Amber77	0:67bd1a7efc59	827	_2_LastSectionAngle_1 = _2_LastSectionAngle;
Amber77	0:67bd1a7efc59	828	_2_LastSectionAngle = _2_SectionAngle;
Amber77	0:67bd1a7efc59	829	LastAngle_3 = Angle_3;
Amber77	0:67bd1a7efc59	830	_3_LastSectionAngle_4 = _3_LastSectionAngle_3;
Amber77	0:67bd1a7efc59	831	_3_LastSectionAngle_3 = _3_LastSectionAngle_2;
Amber77	0:67bd1a7efc59	832	_3_LastSectionAngle_2 = _3_LastSectionAngle_1;
Amber77	0:67bd1a7efc59	833	_3_LastSectionAngle_1 = _3_LastSectionAngle;
Amber77	0:67bd1a7efc59	834	_3_LastSectionAngle = _3_SectionAngle;
Amber77	0:67bd1a7efc59	835	LastAngle_4 = Angle_4;
Amber77	0:67bd1a7efc59	836	_4_LastSectionAngle_4 = _4_LastSectionAngle_3;
Amber77	0:67bd1a7efc59	837	_4_LastSectionAngle_3 = _4_LastSectionAngle_2;
Amber77	0:67bd1a7efc59	838	_4_LastSectionAngle_2 = _4_LastSectionAngle_1;
Amber77	0:67bd1a7efc59	839	_4_LastSectionAngle_1 = _4_LastSectionAngle;
Amber77	0:67bd1a7efc59	840	_4_LastSectionAngle = _4_SectionAngle;
Amber77	0:67bd1a7efc59	841
Amber77	0:67bd1a7efc59	842	if (Angle_1>=0 && Angle_3>=0)
Amber77	0:67bd1a7efc59	843	{
Amber77	0:67bd1a7efc59	844	Angle_Y = (Angle_1 + Angle_3)*0.5;
Amber77	0:67bd1a7efc59	845	}
Amber77	0:67bd1a7efc59	846	else if(Angle_1>=0 && Angle_3<=0)
Amber77	0:67bd1a7efc59	847	{
Amber77	0:67bd1a7efc59	848	Angle_Y = (Angle_1 + abs(Angle_3))*0.5;
Amber77	0:67bd1a7efc59	849	}
Amber77	0:67bd1a7efc59	850	else if(Angle_1<=0 && Angle_3>=0)
Amber77	0:67bd1a7efc59	851	{
Amber77	0:67bd1a7efc59	852	Angle_Y = -(abs(Angle_1) + Angle_3)*0.5;
Amber77	0:67bd1a7efc59	853	}
Amber77	0:67bd1a7efc59	854	else if(Angle_1<=0 && Angle_3<=0)
Amber77	0:67bd1a7efc59	855	{
Amber77	0:67bd1a7efc59	856	Angle_Y = (Angle_1 + Angle_3)*0.5;
Amber77	0:67bd1a7efc59	857	}
Amber77	0:67bd1a7efc59	858
Amber77	0:67bd1a7efc59	859	if (Angle_2>=0 && Angle_4>=0)
Amber77	0:67bd1a7efc59	860	{
Amber77	0:67bd1a7efc59	861	Angle_X = (Angle_2 + Angle_4)*0.5;
Amber77	0:67bd1a7efc59	862	}
Amber77	0:67bd1a7efc59	863	else if (Angle_2>=0 && Angle_4<=0)
Amber77	0:67bd1a7efc59	864	{
Amber77	0:67bd1a7efc59	865	Angle_X = (Angle_2 + abs(Angle_4))*0.5;
Amber77	0:67bd1a7efc59	866	}
Amber77	0:67bd1a7efc59	867	else if (Angle_2<=0 && Angle_4>=0)
Amber77	0:67bd1a7efc59	868	{
Amber77	0:67bd1a7efc59	869	Angle_X = -(abs(Angle_2) + Angle_4)*0.5;
Amber77	0:67bd1a7efc59	870	}
Amber77	0:67bd1a7efc59	871	else if (Angle_2<=0 && Angle_4<=0)
Amber77	0:67bd1a7efc59	872	{
Amber77	0:67bd1a7efc59	873	Angle_X = (Angle_2 + Angle_4)*0.5;
Amber77	0:67bd1a7efc59	874	}
Amber77	0:67bd1a7efc59	875
Amber77	0:67bd1a7efc59	876	}
Amber77	0:67bd1a7efc59	877
Amber77	0:67bd1a7efc59	878	//**************************PWM_commmand_transformation**********************************//
Amber77	0:67bd1a7efc59	879	//**************************PWM_commmand_transformation**********************************//
Amber77	0:67bd1a7efc59	880	double PWM_commmand_transformation( double Control_AngVel_Value )
Amber77	0:67bd1a7efc59	881	{
Amber77	0:67bd1a7efc59	882	double Control_PWM_Value = 0;
Amber77	0:67bd1a7efc59	883	if( Control_AngVel_Value > 0 )
Amber77	0:67bd1a7efc59	884	{
Amber77	0:67bd1a7efc59	885	Control_AngVel_Value = Control_AngVel_Value;
Amber77	0:67bd1a7efc59	886	}
Amber77	0:67bd1a7efc59	887	else if( Control_AngVel_Value < 0 )
Amber77	0:67bd1a7efc59	888	{
Amber77	0:67bd1a7efc59	889	Control_AngVel_Value = -Control_AngVel_Value;
Amber77	0:67bd1a7efc59	890	}
Amber77	0:67bd1a7efc59	891	if( Control_AngVel_Value > 325)
Amber77	0:67bd1a7efc59	892	{
Amber77	0:67bd1a7efc59	893	if( Control_AngVel_Value < 466 )
Amber77	0:67bd1a7efc59	894	{
Amber77	0:67bd1a7efc59	895	if( Control_AngVel_Value < 393 )
Amber77	0:67bd1a7efc59	896	{
Amber77	0:67bd1a7efc59	897	Control_PWM_Value = Control_AngVel_Value/651.6 ; //0.5~0.6
Amber77	0:67bd1a7efc59	898	}
Amber77	0:67bd1a7efc59	899	else
Amber77	0:67bd1a7efc59	900	{
Amber77	0:67bd1a7efc59	901	Control_PWM_Value = Control_AngVel_Value/658.8; //0.6~0.7
Amber77	0:67bd1a7efc59	902	}
Amber77	0:67bd1a7efc59	903	}
Amber77	0:67bd1a7efc59	904	else
Amber77	0:67bd1a7efc59	905	{
Amber77	0:67bd1a7efc59	906	if( Control_AngVel_Value < 523 )
Amber77	0:67bd1a7efc59	907	{
Amber77	0:67bd1a7efc59	908	Control_PWM_Value = Control_AngVel_Value/658.39; //0.7~0.8
Amber77	0:67bd1a7efc59	909	}
Amber77	0:67bd1a7efc59	910	else
Amber77	0:67bd1a7efc59	911	{
Amber77	0:67bd1a7efc59	912	if( Control_AngVel_Value < 588 )
Amber77	0:67bd1a7efc59	913	{
Amber77	0:67bd1a7efc59	914	Control_PWM_Value = Control_AngVel_Value/652.36; //0.8~0.9
Amber77	0:67bd1a7efc59	915	}
Amber77	0:67bd1a7efc59	916	else
Amber77	0:67bd1a7efc59	917	{
Amber77	0:67bd1a7efc59	918	Control_PWM_Value = Control_AngVel_Value/655.11; //0.9~1
Amber77	0:67bd1a7efc59	919	}
Amber77	0:67bd1a7efc59	920	}
Amber77	0:67bd1a7efc59	921	}
Amber77	0:67bd1a7efc59	922	}
Amber77	0:67bd1a7efc59	923	else if( Control_AngVel_Value < 325)
Amber77	0:67bd1a7efc59	924	{
Amber77	0:67bd1a7efc59	925	if( Control_AngVel_Value < 40 )
Amber77	0:67bd1a7efc59	926	{
Amber77	0:67bd1a7efc59	927	Control_PWM_Value = Control_AngVel_Value/533.3; //0~0.075
Amber77	0:67bd1a7efc59	928	}
Amber77	0:67bd1a7efc59	929	else
Amber77	0:67bd1a7efc59	930	{
Amber77	0:67bd1a7efc59	931	if( Control_AngVel_Value < 59 )
Amber77	0:67bd1a7efc59	932	{
Amber77	0:67bd1a7efc59	933	Control_PWM_Value = Control_AngVel_Value/560.65; //0.1~0.15
Amber77	0:67bd1a7efc59	934	}
Amber77	0:67bd1a7efc59	935	else
Amber77	0:67bd1a7efc59	936	{
Amber77	0:67bd1a7efc59	937	if( Control_AngVel_Value < 131 )
Amber77	0:67bd1a7efc59	938	{
Amber77	0:67bd1a7efc59	939	Control_PWM_Value = Control_AngVel_Value/638.3; //0.15~0.2
Amber77	0:67bd1a7efc59	940	}
Amber77	0:67bd1a7efc59	941	else
Amber77	0:67bd1a7efc59	942	{
Amber77	0:67bd1a7efc59	943	if( Control_AngVel_Value < 197 )
Amber77	0:67bd1a7efc59	944	{
Amber77	0:67bd1a7efc59	945	Control_PWM_Value = Control_AngVel_Value/651.6; //0.2~0.3
Amber77	0:67bd1a7efc59	946	}
Amber77	0:67bd1a7efc59	947	else
Amber77	0:67bd1a7efc59	948	{
Amber77	0:67bd1a7efc59	949	if( Control_AngVel_Value < 263 )
Amber77	0:67bd1a7efc59	950	{
Amber77	0:67bd1a7efc59	951	Control_PWM_Value = Control_AngVel_Value/654.16; //0.3~0.4
Amber77	0:67bd1a7efc59	952	}
Amber77	0:67bd1a7efc59	953	else
Amber77	0:67bd1a7efc59	954	{
Amber77	0:67bd1a7efc59	955	Control_PWM_Value = Control_AngVel_Value/652.5; //0.4~0.5
Amber77	0:67bd1a7efc59	956	}
Amber77	0:67bd1a7efc59	957	}
Amber77	0:67bd1a7efc59	958	}
Amber77	0:67bd1a7efc59	959	}
Amber77	0:67bd1a7efc59	960	}
Amber77	0:67bd1a7efc59	961	}
Amber77	0:67bd1a7efc59	962	return Control_PWM_Value;
Amber77	0:67bd1a7efc59	963	}
Amber77	0:67bd1a7efc59	964
Amber77	0:67bd1a7efc59	965	//**************************PIDcontrol_compute_velocity**********************************//
Amber77	0:67bd1a7efc59	966	//**************************PIDcontrol_compute_velocity**********************************//
Amber77	0:67bd1a7efc59	967	void PIDcontrol_compute_velocity(void)
Amber77	0:67bd1a7efc59	968	{
Amber77	0:67bd1a7efc59	969	if(command_AngularVel_X >= 0)
Amber77	0:67bd1a7efc59	970	{
Amber77	0:67bd1a7efc59	971	Control_X1_CW_CCW = 1;
Amber77	0:67bd1a7efc59	972	Control_X2_CW_CCW = 0;
Amber77	0:67bd1a7efc59	973	Command_AngularVel_X = command_AngularVel_X;
Amber77	0:67bd1a7efc59	974	}
Amber77	0:67bd1a7efc59	975	else
Amber77	0:67bd1a7efc59	976	{
Amber77	0:67bd1a7efc59	977	Control_X1_CW_CCW = 0;
Amber77	0:67bd1a7efc59	978	Control_X2_CW_CCW = 1;
Amber77	0:67bd1a7efc59	979	Command_AngularVel_X = -command_AngularVel_X;
Amber77	0:67bd1a7efc59	980	}
Amber77	0:67bd1a7efc59	981	if(command_AngularVel_Y >= 0)
Amber77	0:67bd1a7efc59	982	{
Amber77	0:67bd1a7efc59	983	Control_Y1_CW_CCW = 1;
Amber77	0:67bd1a7efc59	984	Control_Y2_CW_CCW = 0;
Amber77	0:67bd1a7efc59	985	Command_AngularVel_Y = command_AngularVel_Y;
Amber77	0:67bd1a7efc59	986	}
Amber77	0:67bd1a7efc59	987	else
Amber77	0:67bd1a7efc59	988	{
Amber77	0:67bd1a7efc59	989	Control_Y1_CW_CCW = 0;
Amber77	0:67bd1a7efc59	990	Control_Y2_CW_CCW = 1;
Amber77	0:67bd1a7efc59	991	Command_AngularVel_Y = -command_AngularVel_Y;
Amber77	0:67bd1a7efc59	992	}
Amber77	0:67bd1a7efc59	993	Now_time_PID=NowTime.read();
Amber77	0:67bd1a7efc59	994	Motor_X.Compute(&Now_time_PID);
Amber77	0:67bd1a7efc59	995	Motor_Y.Compute(&Now_time_PID);
Amber77	0:67bd1a7efc59	996	// Control_Motor_PWM_X = PWM_commmand_transformation(Control_motor_X);
Amber77	0:67bd1a7efc59	997	// Control_Motor_PWM_Y = PWM_commmand_transformation(Control_motor_Y);
Amber77	0:67bd1a7efc59	998	Control_Motor_PWM_X = PWM_commmand_transformation(command_AngularVel_X);
Amber77	0:67bd1a7efc59	999	Control_Motor_PWM_Y = PWM_commmand_transformation(command_AngularVel_Y);
Amber77	0:67bd1a7efc59	1000	}
Amber77	0:67bd1a7efc59	1001
Amber77	0:67bd1a7efc59	1002	//**************************Compute_point**********************************//
Amber77	0:67bd1a7efc59	1003	//**************************Compute_point**********************************//
Amber77	0:67bd1a7efc59	1004	void LQR_control_compute(void)
Amber77	0:67bd1a7efc59	1005	{
Amber77	0:67bd1a7efc59	1006	Diff_Roll = (Roll - Roll_last);
Amber77	0:67bd1a7efc59	1007	Diff_Pitch = (Pitch - Pitch_last);
Amber77	0:67bd1a7efc59	1008	Integ_Roll += Roll;
Amber77	0:67bd1a7efc59	1009	//printf("Diff_Roll:%.3f\n",Diff_Roll);
Amber77	0:67bd1a7efc59	1010	Integ_Pitch += Pitch;
Amber77	0:67bd1a7efc59	1011	Roll_last = Roll;
Amber77	0:67bd1a7efc59	1012	Pitch_last = Pitch;
Amber77	0:67bd1a7efc59	1013
Amber77	0:67bd1a7efc59	1014	//Diff_Roll =0;
Amber77	0:67bd1a7efc59	1015	//Roll -= 2.45;
Amber77	0:67bd1a7efc59	1016	//Pitch -=2.5;
Amber77	0:67bd1a7efc59	1017	Diff_x = x_now - x_trajectory;
Amber77	0:67bd1a7efc59	1018	Diff_y = y_now - y_trajectory;
Amber77	0:67bd1a7efc59	1019	dot_diff_x = Diff_x - Diff_x_pre;
Amber77	0:67bd1a7efc59	1020	dot_diff_y = Diff_y - Diff_y_pre;
Amber77	0:67bd1a7efc59	1021	Integ_x += Diff_x;
Amber77	0:67bd1a7efc59	1022	Integ_y += Diff_y;
Amber77	0:67bd1a7efc59	1023
Amber77	0:67bd1a7efc59	1024	//x_pre_1 = x_now;
Amber77	0:67bd1a7efc59	1025	//y_pre_1 = y_now;
Amber77	0:67bd1a7efc59	1026	Diff_x_pre = Diff_x;
Amber77	0:67bd1a7efc59	1027	Diff_y_pre = Diff_y;
Amber77	0:67bd1a7efc59	1028
Amber77	0:67bd1a7efc59	1029	// u_x = Ka(Roll)+ KavDiff_Roll + KtDiff_x + Kvdot_diff_x + KI_xy*Integ_x;
Amber77	0:67bd1a7efc59	1030	// u_y = Ka(Pitch)+KavDiff_Pitch + Kt_yDiff_y + Kv_ydot_diff_y + KI_xy_y*Integ_y;
Amber77	0:67bd1a7efc59	1031	u_x = Ka(Roll)+(Integ_RollKii) + KavDiff_Roll + KtDiff_x + Kvdot_diff_x + KI_xyInteg_x;
Amber77	0:67bd1a7efc59	1032	// u_x = Ka(Roll)+(Integ_RollKii) + Kav*Diff_Roll;
Amber77	0:67bd1a7efc59	1033	//u_x = Ka(Roll)+(Integ_RollKii) + KavDiff_Roll - Kt(10) - Kv*0;
Amber77	0:67bd1a7efc59	1034	// u_x = Ka*(Roll);
Amber77	0:67bd1a7efc59	1035	u_y = Ka(Pitch)+(Integ_PitchKii) + KavDiff_Pitch + Kt_yDiff_y + Kv_ydot_diff_y + KI_xy_yInteg_y;
Amber77	0:67bd1a7efc59	1036	// u_y = Ka(Pitch)+(Integ_RollKii) + Kav*Diff_Roll;
Amber77	0:67bd1a7efc59	1037	//u_y = Ka(Pitch)+(Integ_PitchKii) + KavDiff_Pitch - Kt0 - Kv*0;
Amber77	0:67bd1a7efc59	1038	// u_y = Ka*(Pitch);
Amber77	0:67bd1a7efc59	1039	if(u_x > 580)
Amber77	0:67bd1a7efc59	1040	{
Amber77	0:67bd1a7efc59	1041	u_x = 580;
Amber77	0:67bd1a7efc59	1042	}
Amber77	0:67bd1a7efc59	1043	else if(u_x < -580)
Amber77	0:67bd1a7efc59	1044	{
Amber77	0:67bd1a7efc59	1045	u_x = -580;
Amber77	0:67bd1a7efc59	1046	}
Amber77	0:67bd1a7efc59	1047	if(u_y > 580 )
Amber77	0:67bd1a7efc59	1048	{
Amber77	0:67bd1a7efc59	1049	u_y = 580;
Amber77	0:67bd1a7efc59	1050	}
Amber77	0:67bd1a7efc59	1051	else if(u_y < -580)
Amber77	0:67bd1a7efc59	1052	{
Amber77	0:67bd1a7efc59	1053	u_y = -580;
Amber77	0:67bd1a7efc59	1054	}
Amber77	0:67bd1a7efc59	1055	Vx = u_x;
Amber77	0:67bd1a7efc59	1056	Vy = u_y;
Amber77	0:67bd1a7efc59	1057	Wz = Yaw;
Amber77	0:67bd1a7efc59	1058
Amber77	0:67bd1a7efc59	1059	// Vx = 100;
Amber77	0:67bd1a7efc59	1060	// Vy = 100;
Amber77	0:67bd1a7efc59	1061	// Wz =0;
Amber77	0:67bd1a7efc59	1062	command_AngularVel_X = Vx+Kz*Wz;
Amber77	0:67bd1a7efc59	1063	command_AngularVel_Y = Vy+Kz*Wz;
Amber77	0:67bd1a7efc59	1064	}
Amber77	0:67bd1a7efc59	1065
Amber77	0:67bd1a7efc59	1066	//***********************************************************//
Amber77	0:67bd1a7efc59	1067	//
Amber77	0:67bd1a7efc59	1068	////若要前進，則在x或y加減p_set。根據底盤的正負標識，來決定加或減。
Amber77	0:67bd1a7efc59	1069	//
Amber77	0:67bd1a7efc59	1070	//
Amber77	0:67bd1a7efc59	1071	// /v_dx_new=0.4(1Kp3(err_tiltX)+(1.9Kd31)(err_gyroX) +(x)Kp319 +vel_xKd315-0v_dx);
Amber77	0:67bd1a7efc59	1072	// v_dy_new=0.4(1Kp3(err_tiltY)+(1.9Kd31)(err_gyroY) +(y)Kp319 +(vel_y)Kd315-0v_dy);/
Amber77	0:67bd1a7efc59	1073	// // x_u=12.51.6666v_d_gain(1Kp3(err_tiltX)+(1.9Kd31)(err_gyroX) +(x)Kp30 +vel_xKd30-0*x_iu);
Amber77	0:67bd1a7efc59	1074	// // y_u=12.51.6666v_d_gain(1Kp3(err_tiltY)+(1.9Kd31)(err_gyroY) +(y)Kp30 +vel_yKd30-0*y_iu);
Amber77	0:67bd1a7efc59	1075	// /x_u=6.25v_d_gain( -Kp3err_tiltX+(-1.9Kd3)err_gyroX +(-0.0010Kp3)x +(-0.0180Kp3)vel_x-0*x_iu);
Amber77	0:67bd1a7efc59	1076	// y_u=6.25v_d_gain( -Kp3err_tiltY+(-1.9Kd3)err_gyroY +(-0.0010Kp3)y +(-0.0180Kp3)vel_y-0y_iu);
Amber77	0:67bd1a7efc59	1077	//*/
Amber77	0:67bd1a7efc59	1078	// // integral U for PI close loop
Amber77	0:67bd1a7efc59	1079	// x_iu=x_u*T2+x_iu_old; //integral value of v_dx
Amber77	0:67bd1a7efc59	1080	// y_iu=y_u*T2+y_iu_old;
Amber77	0:67bd1a7efc59	1081	//
Amber77	0:67bd1a7efc59	1082	// x_iu_old=x_iu;
Amber77	0:67bd1a7efc59	1083	// y_iu_old=y_iu;
Amber77	0:67bd1a7efc59	1084	//
Amber77	0:67bd1a7efc59	1085	// // dV cmd(U cmd) PI close loop (like low-pass filter multip a gain value)
Amber77	0:67bd1a7efc59	1086	// C_theta1_d=(8)((x_u-vel_x0.3C_theta_gain1)+(C_theta_gain2)(x_iu0.24- x0.3 )+fc*vel_x);
Amber77	0:67bd1a7efc59	1087	// C_theta2_d=(8)((y_u-vel_y0.3C_theta_gain1)+(C_theta_gain2)(y_iu0.24- y0.3 )+fc*vel_y);
Amber77	0:67bd1a7efc59	1088	//
Amber77	0:67bd1a7efc59	1089	// // Vx = integral dVx
Amber77	0:67bd1a7efc59	1090	//
Amber77	0:67bd1a7efc59	1091	// C_theta1=C_theta1_d*T2+C_theta1_old;
Amber77	0:67bd1a7efc59	1092	// // limit
Amber77	0:67bd1a7efc59	1093	// C_theta1= C_theta1> (5000) ? (5000): C_theta1;
Amber77	0:67bd1a7efc59	1094	// C_theta1= C_theta1< (-5000) ? (-5000): C_theta1;
Amber77	0:67bd1a7efc59	1095	//
Amber77	0:67bd1a7efc59	1096	// C_theta1_old=C_theta1;
Amber77	0:67bd1a7efc59	1097	// // integral Vx
Amber77	0:67bd1a7efc59	1098	// C_theta1_i=C_theta1*T2+C_theta1_i;
Amber77	0:67bd1a7efc59	1099	//
Amber77	0:67bd1a7efc59	1100	// // Vy = integral dVy
Amber77	0:67bd1a7efc59	1101	// C_theta2=C_theta2_d*T2+C_theta2_old;
Amber77	0:67bd1a7efc59	1102	// // limit
Amber77	0:67bd1a7efc59	1103	// C_theta2= C_theta2>(5000) ? (5000): C_theta2;
Amber77	0:67bd1a7efc59	1104	// C_theta2= C_theta2<(-5000) ? (-5000):C_theta2;
Amber77	0:67bd1a7efc59	1105	//
Amber77	0:67bd1a7efc59	1106	// C_theta2_old=C_theta2;
Amber77	0:67bd1a7efc59	1107	// // integral Vy
Amber77	0:67bd1a7efc59	1108	// C_theta2_i=C_theta2*T2+C_theta2_i;
Amber77	0:67bd1a7efc59	1109	//
Amber77	0:67bd1a7efc59	1110	// // speed PI LOOP control
Amber77	0:67bd1a7efc59	1111	//
Amber77	0:67bd1a7efc59	1112	// C_thetaX=Kcp(C_theta1-vel_xCgain1)+Kci(C_theta1_i-xCgain2); //X axis
Amber77	0:67bd1a7efc59	1113	// C_thetaY=Kcp(C_theta2-vel_yCgain1)+Kci(C_theta2_i-yCgain2); //Y axis
Amber77	0:67bd1a7efc59	1114	//
Amber77	0:67bd1a7efc59	1115	// #endif
Amber77	0:67bd1a7efc59	1116	// #if Testmode == 1 // nonlinear
Amber77	0:67bd1a7efc59	1117	//
Amber77	0:67bd1a7efc59	1118	////########### x 方向之控制器#############
Amber77	0:67bd1a7efc59	1119	// r1=1;
Amber77	0:67bd1a7efc59	1120	// r2=1;
Amber77	0:67bd1a7efc59	1121	// err_tilt= 0+tilt20.0006086;/tilt_out;傾斜儀資料 */
Amber77	0:67bd1a7efc59	1122	// err_gyro= 0+gyro2;/陀螺儀資料/
Amber77	0:67bd1a7efc59	1123	// sin_t=sin(err_tilt);
Amber77	0:67bd1a7efc59	1124	// cos_t=cos(err_tilt);
Amber77	0:67bd1a7efc59	1125	// err_tilt_i=err_tilt_i+err_tilt*0.1;
Amber77	0:67bd1a7efc59	1126	// x_i=x_i+x*0.001;
Amber77	0:67bd1a7efc59	1127	// //err_tilt_i+=err_tilt;
Amber77	0:67bd1a7efc59	1128	// //x_i+=x;
Amber77	0:67bd1a7efc59	1129	// delta=187.5-125.3*cos_t;
Amber77	0:67bd1a7efc59	1130	// A=(5.51+33.75*cos_t)/delta;
Amber77	0:67bd1a7efc59	1131	// B=-((125.3sin_tcos_t)/delta)err_gyroerr_gyro+182.2*sin_t/delta;
Amber77	0:67bd1a7efc59	1132	//// B=-((125.3sin_tcos_t)/delta)err_gyroerr_gyro+182.2sin_t/delta-5.5091((err_gyro+vel_x/0.11))/187.5-125.3cos_tcos_t;
Amber77	0:67bd1a7efc59	1133	// C=Kp3(eta1-Kp3err_tilt-Ki3err_tilt_i)+Ki3err_tilt;
Amber77	0:67bd1a7efc59	1134	// D=-(5.51+33.75*cos_t)/delta;
Amber77	0:67bd1a7efc59	1135	// E=126.4/deltaerr_gyroerr_gyrosin_t-1637/deltasin_t*cos_t;
Amber77	0:67bd1a7efc59	1136	// F=Kp3(eta2-Kp3x-Ki3x_i)+Ki3x;
Amber77	0:67bd1a7efc59	1137	////F=Kp3(eta2-Kp3err_tilt-Ki30)+Ki30;
Amber77	0:67bd1a7efc59	1138	// eta1=err_gyro+Kp3err_tilt+Ki3err_tilt_i;
Amber77	0:67bd1a7efc59	1139	// eta2=vel_x+Kp3x+Ki3x_i;
Amber77	0:67bd1a7efc59	1140	////eta2=0;
Amber77	0:67bd1a7efc59	1141	// s=r1eta1+r2eta2;
Amber77	0:67bd1a7efc59	1142	// if(s>=1) sgns=1;
Amber77	0:67bd1a7efc59	1143	// else if(s<=-1) sgns=-1;
Amber77	0:67bd1a7efc59	1144	// else sgns=s;
Amber77	0:67bd1a7efc59	1145	//
Amber77	0:67bd1a7efc59	1146	// if (s*abs(vel_x)>=1) sgns_c=1;
Amber77	0:67bd1a7efc59	1147	// else if(s*abs(vel_x)<=-1) sgns_c=-1;
Amber77	0:67bd1a7efc59	1148	// else sgns_c=s*abs(vel_x);
Amber77	0:67bd1a7efc59	1149	////########### y 方向之控制器#############
Amber77	0:67bd1a7efc59	1150	// err_tilt2= 0+tilt50.0006086;/tilt_out;傾斜儀資料 */
Amber77	0:67bd1a7efc59	1151	// err_gyro2= 0+gyro5;/陀螺儀資料/
Amber77	0:67bd1a7efc59	1152	// sin_t2=sin(err_tilt2);
Amber77	0:67bd1a7efc59	1153	// cos_t2=cos(err_tilt2);
Amber77	0:67bd1a7efc59	1154	// err_tilt2_i=err_tilt2_i+err_tilt2*0.1;
Amber77	0:67bd1a7efc59	1155	// y_i=y_i+y*0.001;
Amber77	0:67bd1a7efc59	1156	////err_tilt2_i+=err_tilt2;
Amber77	0:67bd1a7efc59	1157	////y_i+=y;
Amber77	0:67bd1a7efc59	1158	// delta2=187.5-125.3*cos_t2;
Amber77	0:67bd1a7efc59	1159	// A2=(5.51+33.75*cos_t2)/delta2;
Amber77	0:67bd1a7efc59	1160	// B2=-((125.3sin_t2cos_t2)/delta2)err_gyro2err_gyro2+182.2*sin_t2/delta2;
Amber77	0:67bd1a7efc59	1161	// C2=Kp3(eta3-Kp3err_tilt2-Ki3err_tilt2_i)+Ki3err_tilt2;
Amber77	0:67bd1a7efc59	1162	// D2=-(5.51+33.75*cos_t2)/delta2;
Amber77	0:67bd1a7efc59	1163	// E2=126.4/delta2err_gyro2err_gyro2sin_t2-1637/delta2sin_t2*cos_t2;
Amber77	0:67bd1a7efc59	1164	// F2=Kp3(eta4-Kp3y-Ki3y_i)+Ki3y;
Amber77	0:67bd1a7efc59	1165	////F2=Kp3(eta4-Kp3err_tilt2-Ki30)+Ki30;
Amber77	0:67bd1a7efc59	1166	// eta3=err_gyro2+Kp3err_tilt2+Ki3err_tilt2_i;
Amber77	0:67bd1a7efc59	1167	// eta4=vel_y+Kp3y+Ki3y_i;
Amber77	0:67bd1a7efc59	1168	////eta4=0;
Amber77	0:67bd1a7efc59	1169	// s2=r1eta3+r2eta4;
Amber77	0:67bd1a7efc59	1170	// if(s2>=1) sgns2=1;
Amber77	0:67bd1a7efc59	1171	// else if(s2<=-1) sgns2=-1;
Amber77	0:67bd1a7efc59	1172	// else sgns2=s2;
Amber77	0:67bd1a7efc59	1173	//
Amber77	0:67bd1a7efc59	1174	// if(s2*abs(vel_y)>=1) sgns2_c=1;
Amber77	0:67bd1a7efc59	1175	// else if(s2*abs(vel_y)<=-1) sgns2_c=-1;
Amber77	0:67bd1a7efc59	1176	// else sgns2_c=s2*abs(vel_y);
Amber77	0:67bd1a7efc59	1177	//
Amber77	0:67bd1a7efc59	1178	////若要前進，則在x或y加減p_set。根據底盤的正負標識，來決定加或減。
Amber77	0:67bd1a7efc59	1179	// C_theta1_new=((r1B+r1C+r2E+r2F)+s+sgns+sgns_c)/(r1A+r2D);
Amber77	0:67bd1a7efc59	1180	// C_theta2_new=((r1B2+r1C2+r2E2+r2F2)+s2+sgns2+sgns2_c)/(r1A2+r2D2);
Amber77	0:67bd1a7efc59	1181	// }
Amber77	0:67bd1a7efc59	1182	// i++;
Amber77	0:67bd1a7efc59	1183	//
Amber77	0:67bd1a7efc59	1184	// C_theta1=C_theta1_new*0.00050+C_theta1_old;
Amber77	0:67bd1a7efc59	1185	// C_theta1_old=C_theta1;
Amber77	0:67bd1a7efc59	1186	//
Amber77	0:67bd1a7efc59	1187	//
Amber77	0:67bd1a7efc59	1188	// C_theta2=C_theta2_new*0.00050+C_theta2_old;
Amber77	0:67bd1a7efc59	1189	// C_theta2_old=C_theta2;
Amber77	0:67bd1a7efc59	1190	//
Amber77	0:67bd1a7efc59	1191	// C_theta1= C_theta1>(42143)?(42143): C_theta1;
Amber77	0:67bd1a7efc59	1192	// C_theta1= C_theta1<-1(42143)?-1(42143): C_theta1;
Amber77	0:67bd1a7efc59	1193	// C_theta2= C_theta2>(42143)?(42143): C_theta2;
Amber77	0:67bd1a7efc59	1194	// C_theta2=C_theta2<-1(42143)?-1(42143):C_theta2;
Amber77	0:67bd1a7efc59	1195	//
Amber77	0:67bd1a7efc59	1196	// #endif
Amber77	0:67bd1a7efc59	1197
Amber77	0:67bd1a7efc59	1198	//***********************************************************//
Amber77	0:67bd1a7efc59	1199
Amber77	0:67bd1a7efc59	1200
Amber77	0:67bd1a7efc59	1201	void MeasureRobotAttitudeAngle(void)
Amber77	0:67bd1a7efc59	1202	{
Amber77	0:67bd1a7efc59	1203	dt = t_MeasureRobotAttitudeAngle;
Amber77	0:67bd1a7efc59	1204	imu.read_all();
Amber77	0:67bd1a7efc59	1205	Mag_Complentary_Filter(dt,imu.Magnetometer);
Amber77	0:67bd1a7efc59	1206	Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
Amber77	0:67bd1a7efc59	1207	/x_now = x_pre_1 + (1/(dtdt))(ax_now-2ax_pre_1+ax_pre_2);
Amber77	0:67bd1a7efc59	1208	y_now = y_pre_1 + (1/(dtdt))(ay_now-2*ay_pre_1+ay_pre_2);
Amber77	0:67bd1a7efc59	1209	x_pre_1 = x_now;
Amber77	0:67bd1a7efc59	1210	y_pre_1 = y_now;*/
Amber77	0:67bd1a7efc59	1211	x_now = r_grounderpi(Angle_X/180);
Amber77	0:67bd1a7efc59	1212	y_now = r_grounderpi(Angle_Y/180);
Amber77	0:67bd1a7efc59	1213	// x_now = r_ballpi(Angle_X/180);
Amber77	0:67bd1a7efc59	1214	// y_now = r_ballpi(Angle_Y/180);
Amber77	0:67bd1a7efc59	1215	do_measure_index++;
Amber77	0:67bd1a7efc59	1216	}
Amber77	0:67bd1a7efc59	1217	//void Trajectory_tracking(void)
Amber77	0:67bd1a7efc59	1218	//{
Amber77	0:67bd1a7efc59	1219	// double t_trajectory = NowTime.read();
Amber77	0:67bd1a7efc59	1220	// if( RunTime>=0 &&( RunTime<=10))
Amber77	0:67bd1a7efc59	1221	// {
Amber77	0:67bd1a7efc59	1222	// //x_trajectory = t_trajectory * (0.5);
Amber77	0:67bd1a7efc59	1223	// x_trajectory = 0;
Amber77	0:67bd1a7efc59	1224	// y_trajectory = 0;
Amber77	0:67bd1a7efc59	1225	// }
Amber77	0:67bd1a7efc59	1226	// else if( RunTime>10 )
Amber77	0:67bd1a7efc59	1227	// {
Amber77	0:67bd1a7efc59	1228	// x_trajectory = 0;
Amber77	0:67bd1a7efc59	1229	// y_trajectory = 0;
Amber77	0:67bd1a7efc59	1230	// }
Amber77	0:67bd1a7efc59	1231	//}
Amber77	0:67bd1a7efc59	1232	//**************************main function**********************************//
Amber77	0:67bd1a7efc59	1233	int main()
Amber77	0:67bd1a7efc59	1234	{
Amber77	0:67bd1a7efc59	1235	// pc.baud(115200);
Amber77	0:67bd1a7efc59	1236	pc.baud(230400);
Amber77	0:67bd1a7efc59	1237
Amber77	0:67bd1a7efc59	1238	//**************************Motor driver declare**********************************//
Amber77	0:67bd1a7efc59	1239	//ExtInt = 0; //number high, voltage high.
Amber77	0:67bd1a7efc59	1240	//Control_stopRun=0;
Amber77	0:67bd1a7efc59	1241	// Control_brake=1;
Amber77	0:67bd1a7efc59	1242	Control_AR=1;
Amber77	0:67bd1a7efc59	1243	Control_H_F=1;
Amber77	0:67bd1a7efc59	1244	H_F.write(Control_H_F); // control_H_F must equal to 1
Amber77	0:67bd1a7efc59	1245	// StopRun.write(Control_stopRun);
Amber77	0:67bd1a7efc59	1246	// Brake.write(Control_brake);
Amber77	0:67bd1a7efc59	1247	// AR.write(Control_AR);
Amber77	0:67bd1a7efc59	1248	//**************************Angle Sensor initialization**********************************//
Amber77	0:67bd1a7efc59	1249	if(imu.init(1,BITS_DLPF_CFG_188HZ))
Amber77	0:67bd1a7efc59	1250	{ //INIT the mpu9250
Amber77	0:67bd1a7efc59	1251	// pc.printf("\nCouldn't initialize MPU9250 via SPI!");
Amber77	0:67bd1a7efc59	1252	}
Amber77	0:67bd1a7efc59	1253	imu.whoami();
Amber77	0:67bd1a7efc59	1254	// wait(0.1);
Amber77	0:67bd1a7efc59	1255	imu.set_gyro_scale(BITS_FS_2000DPS);
Amber77	0:67bd1a7efc59	1256	// wait(0.1);
Amber77	0:67bd1a7efc59	1257	imu.set_acc_scale(BITS_FS_16G);
Amber77	0:67bd1a7efc59	1258	// wait(0.1);
Amber77	0:67bd1a7efc59	1259	imu.AK8963_whoami();
Amber77	0:67bd1a7efc59	1260	// wait(0.1);
Amber77	0:67bd1a7efc59	1261	imu.AK8963_calib_Magnetometer();
Amber77	0:67bd1a7efc59	1262	////**************************SD card**********************************//
Amber77	0:67bd1a7efc59	1263	mkdir("/sd/Amber20170822a", 0777); //SD裡面的資料夾叫Amber77,在此做宣告
Amber77	0:67bd1a7efc59	1264	FILE *fp = fopen("/sd/Amber20170822a/RollPitchYaw_y1.csv", "a");
Amber77	0:67bd1a7efc59	1265	//將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv
Amber77	0:67bd1a7efc59	1266	fprintf(fp,"RunTime,Roll,Pitch,Yaw,Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory\n");
Amber77	0:67bd1a7efc59	1267
Amber77	0:67bd1a7efc59	1268	//************************** PWM **********************************//
Amber77	0:67bd1a7efc59	1269	// X_PWM.calibrate(0.00005, 0.080.00005, 10.00005); //20kHZ
Amber77	0:67bd1a7efc59	1270	// Y_PWM.calibrate(0.00005, 0.080.00005, 10.00005); //20kHZ
Amber77	0:67bd1a7efc59	1271	// X_PWM.calibrate(0.02, 00.02, 10.02);
Amber77	0:67bd1a7efc59	1272	// Y_PWM.calibrate(0.02, 00.02, 10.02);
Amber77	0:67bd1a7efc59	1273	X_PWM.calibrate(0.0001, 00.0001, 10.0001); //10kHZ
Amber77	0:67bd1a7efc59	1274	Y_PWM.calibrate(0.0001, 00.0001, 10.0001); //10kHZ
Amber77	0:67bd1a7efc59	1275	//************************** Encoder **********************************//
Amber77	0:67bd1a7efc59	1276	phaseA_1.mode( PullUp );
Amber77	0:67bd1a7efc59	1277	phaseB_1.mode( PullUp );
Amber77	0:67bd1a7efc59	1278	phaseA_2.mode( PullUp );
Amber77	0:67bd1a7efc59	1279	phaseB_2.mode( PullUp );
Amber77	0:67bd1a7efc59	1280	phaseA_3.mode( PullUp );
Amber77	0:67bd1a7efc59	1281	phaseB_3.mode( PullUp );
Amber77	0:67bd1a7efc59	1282	phaseA_4.mode( PullUp );
Amber77	0:67bd1a7efc59	1283	phaseB_4.mode( PullUp );
Amber77	0:67bd1a7efc59	1284	//************************** Create the motor encoder sampler. **********************************//
Amber77	0:67bd1a7efc59	1285	Sample_Motor_encoder.attach_us( &quadratureDecoder, t_quadratureDecoder );
Amber77	0:67bd1a7efc59	1286	//************************** Create the motor encoder sampler. **********************************//
Amber77	0:67bd1a7efc59	1287	Sample_robotAngleSensor.attach( &MeasureRobotAttitudeAngle, t_MeasureRobotAttitudeAngle );
Amber77	0:67bd1a7efc59	1288	//************************** Create the motor angular velocity measurement. **********************************//
Amber77	0:67bd1a7efc59	1289	MeasureAngularVelocity.attach( &getAngular, t_MeasureAngularVelocity );
Amber77	0:67bd1a7efc59	1290	//************************** Create the motor angular velocity PID control. **********************************//
Amber77	0:67bd1a7efc59	1291	PIDcontrol_velocity.attach( &PIDcontrol_compute_velocity, t_PIDcontrol_velocity );
Amber77	0:67bd1a7efc59	1292	//************************** Create the robot LQR control. **********************************//
Amber77	0:67bd1a7efc59	1293	LQR_control.attach( &LQR_control_compute, t_LQR_control );
Amber77	0:67bd1a7efc59	1294	//************************** Trajectory tracking control **********************************//
Amber77	0:67bd1a7efc59	1295	// TrajectoryTracking_control.attach( &Trajectory_tracking, t_trajectory );
Amber77	0:67bd1a7efc59	1296	//************************** Motor settlement **********************************//
Amber77	0:67bd1a7efc59	1297	Motor_X.SetMode(AUTOMATIC);
Amber77	0:67bd1a7efc59	1298	Motor_Y.SetMode(AUTOMATIC);
Amber77	0:67bd1a7efc59	1299	// Command_AngularVel_X = 0;
Amber77	0:67bd1a7efc59	1300	// Command_AngularVel_Y = 0;
Amber77	0:67bd1a7efc59	1301	//************************** Timers start **********************************//
Amber77	0:67bd1a7efc59	1302	NowTime.start();
Amber77	0:67bd1a7efc59	1303	while( 1 )
Amber77	0:67bd1a7efc59	1304	{
Amber77	0:67bd1a7efc59	1305	RunTime = NowTime.read();
Amber77	0:67bd1a7efc59	1306	t_trajectory = NowTime.read();
Amber77	0:67bd1a7efc59	1307	limitSwitchUp = LimitSwitchUp.read();
Amber77	0:67bd1a7efc59	1308	limitSwitchDown = LimitSwitchDown.read();
Amber77	0:67bd1a7efc59	1309	//pc.printf("%d \n",Control_F_R);
Amber77	0:67bd1a7efc59	1310	if (mybutton == 0)
Amber77	0:67bd1a7efc59	1311	{
Amber77	0:67bd1a7efc59	1312	if( Control_F_R == 1)
Amber77	0:67bd1a7efc59	1313	{
Amber77	0:67bd1a7efc59	1314	Control_LiftingStopRun=0;
Amber77	0:67bd1a7efc59	1315	LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop
Amber77	0:67bd1a7efc59	1316	F_R.write(Control_F_R); // 0:turn down 1:turn up
Amber77	0:67bd1a7efc59	1317	Control_stopRun=0;
Amber77	0:67bd1a7efc59	1318	Control_brake=1;
Amber77	0:67bd1a7efc59	1319	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);
Amber77	0:67bd1a7efc59	1320	wait(3);
Amber77	0:67bd1a7efc59	1321	}
Amber77	0:67bd1a7efc59	1322	else if ( Control_F_R == 0)
Amber77	0:67bd1a7efc59	1323	{
Amber77	0:67bd1a7efc59	1324	Control_LiftingStopRun=0;
Amber77	0:67bd1a7efc59	1325	LiftingStopRun.write(Control_LiftingStopRun); // 0:Run 1:Stop
Amber77	0:67bd1a7efc59	1326	F_R.write(Control_F_R); // 0:turn down 1:turn up
Amber77	0:67bd1a7efc59	1327	Control_stopRun=0;
Amber77	0:67bd1a7efc59	1328	Control_brake=1;
Amber77	0:67bd1a7efc59	1329	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);
Amber77	0:67bd1a7efc59	1330	wait(2);
Amber77	0:67bd1a7efc59	1331	Control_stopRun=1;
Amber77	0:67bd1a7efc59	1332	Control_brake=1;
Amber77	0:67bd1a7efc59	1333	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);
Amber77	0:67bd1a7efc59	1334	wait(1);
Amber77	0:67bd1a7efc59	1335	}
Amber77	0:67bd1a7efc59	1336	Control_F_R=1-Control_F_R;
Amber77	0:67bd1a7efc59	1337	wait(0.2);
Amber77	0:67bd1a7efc59	1338	}
Amber77	0:67bd1a7efc59	1339	if(limitSwitchUp == 0)
Amber77	0:67bd1a7efc59	1340	{
Amber77	0:67bd1a7efc59	1341	LiftingStopRun.write(1); // 0:Run 1:Stop
Amber77	0:67bd1a7efc59	1342	F_R.write(1); // 0:turn down 1:turn up
Amber77	0:67bd1a7efc59	1343	}
Amber77	0:67bd1a7efc59	1344	if(limitSwitchDown == 0)
Amber77	0:67bd1a7efc59	1345	{
Amber77	0:67bd1a7efc59	1346	LiftingStopRun.write(1); // 0:Run 1:Stop
Amber77	0:67bd1a7efc59	1347	F_R.write(0); // 0:turn down 1:turn up
Amber77	0:67bd1a7efc59	1348	}
Amber77	0:67bd1a7efc59	1349	if( RunTime>=0 &&( RunTime<=10))
Amber77	0:67bd1a7efc59	1350	{
Amber77	0:67bd1a7efc59	1351	//x_trajectory = t_trajectory * (0.5);
Amber77	0:67bd1a7efc59	1352	//Control_stopRun=0;
Amber77	0:67bd1a7efc59	1353	// Control_brake=1;
Amber77	0:67bd1a7efc59	1354	x_trajectory = 0;
Amber77	0:67bd1a7efc59	1355	y_trajectory = 0;
Amber77	0:67bd1a7efc59	1356	}
Amber77	0:67bd1a7efc59	1357	else if( RunTime>10 && (RunTime <=20 ))
Amber77	0:67bd1a7efc59	1358	{
Amber77	0:67bd1a7efc59	1359	//Control_stopRun=0;
Amber77	0:67bd1a7efc59	1360	// Control_brake=1;
Amber77	0:67bd1a7efc59	1361	x_trajectory = (t_trajectory-10) * (0.05);
Amber77	0:67bd1a7efc59	1362	y_trajectory = 0;
Amber77	0:67bd1a7efc59	1363	}
Amber77	0:67bd1a7efc59	1364	else if( RunTime>=20 )
Amber77	0:67bd1a7efc59	1365	{
Amber77	0:67bd1a7efc59	1366	//Control_stopRun=0;
Amber77	0:67bd1a7efc59	1367	// Control_brake=1;
Amber77	0:67bd1a7efc59	1368	x_trajectory = 0;
Amber77	0:67bd1a7efc59	1369	y_trajectory = 0;
Amber77	0:67bd1a7efc59	1370	}
Amber77	0:67bd1a7efc59	1371
Amber77	0:67bd1a7efc59	1372
Amber77	0:67bd1a7efc59	1373	if( (RunTime-lastTime) > 0.1 )
Amber77	0:67bd1a7efc59	1374	{
Amber77	0:67bd1a7efc59	1375	index_times++;
Amber77	0:67bd1a7efc59	1376	lastTime = RunTime;
Amber77	0:67bd1a7efc59	1377	}
Amber77	0:67bd1a7efc59	1378
Amber77	0:67bd1a7efc59	1379	if( index_times >= 0.1 )
Amber77	0:67bd1a7efc59	1380	{
Amber77	0:67bd1a7efc59	1381	/*pc.printf("x_now: %.3f , y_now: %.3f , x_trajectory: %f , u_y: %f ",x_now,y_now,x_trajectory,u_y);
Amber77	0:67bd1a7efc59	1382	pc.printf(" Roll : %10.3f, Pitch : %10.3f, Yaw : %10.3f \n",
Amber77	0:67bd1a7efc59	1383	Roll,
Amber77	0:67bd1a7efc59	1384	Pitch,
Amber77	0:67bd1a7efc59	1385	Yaw
Amber77	0:67bd1a7efc59	1386	);*/
Amber77	0:67bd1a7efc59	1387	//pc.printf(" %.3f , %.3f , %.3f , %.3f ",x_now,y_now,x_trajectory,y_trajectory);
Amber77	0:67bd1a7efc59	1388	//pc.printf("RunTime=%10.3f \|\| Roll=%10.3f \|\| Pitch=%10.3f \|\| Yaw=%10.3f \n",RunTime, Roll, Pitch, Yaw);
Amber77	0:67bd1a7efc59	1389	// 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",
Amber77	0:67bd1a7efc59	1390	// Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_now,y_now,Angle_X,Angle_Y);
Amber77	0:67bd1a7efc59	1391	pc.printf("RunTime=%.3f \|\| Roll=%.3f \|\| Pitch=%.3f \|\| Yaw=%.3f \n",RunTime, Roll, Pitch, Yaw);
Amber77	0:67bd1a7efc59	1392	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",
Amber77	0:67bd1a7efc59	1393	Vx,Vy,Control_Motor_PWM_X,Control_Motor_PWM_Y,x_trajectory,y_trajectory,Angle_X,Angle_Y);
Amber77	0:67bd1a7efc59	1394	//pc.printf(" Now_angularVelocity : %.3f ",Now_angularVelocity);
Amber77	0:67bd1a7efc59	1395	// pc.printf(",RunTime : %.3f ", RunTime );
Amber77	0:67bd1a7efc59	1396	// pc.printf(",control_PWM_Value : %.3f \n", control_PWM_Value);
Amber77	0:67bd1a7efc59	1397	index_times = 0;
Amber77	0:67bd1a7efc59	1398	//if(fp == NULL)
Amber77	0:67bd1a7efc59	1399	// {
Amber77	0:67bd1a7efc59	1400	// error("Could not open file for write\n");
Amber77	0:67bd1a7efc59	1401	// }
Amber77	0:67bd1a7efc59	1402	// //fprintf(fp,"%.3f,%.3f,%.3f\n", RunTime,Now_angularVelocity,control_PWM_Value);
Amber77	0:67bd1a7efc59	1403	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);
Amber77	0:67bd1a7efc59	1404	}
Amber77	0:67bd1a7efc59	1405	// Lifting(Control_LiftingStopRun, Control_F_R , Control_H_F);
Amber77	0:67bd1a7efc59	1406	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);
Amber77	0:67bd1a7efc59	1407	//wait(0.1);
Amber77	0:67bd1a7efc59	1408	// if(!mybutton)
Amber77	0:67bd1a7efc59	1409	// {
Amber77	0:67bd1a7efc59	1410	// StopRun.write(1);
Amber77	0:67bd1a7efc59	1411	// Brake.write(1);
Amber77	0:67bd1a7efc59	1412	// X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77	0:67bd1a7efc59	1413	// X2_CW_CCW.write(0);
Amber77	0:67bd1a7efc59	1414	// Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77	0:67bd1a7efc59	1415	// Y2_CW_CCW.write(0);
Amber77	0:67bd1a7efc59	1416	// X_PWM.write(0); // clockwise:0 counterclockwise:1
Amber77	0:67bd1a7efc59	1417	// Y_PWM.write(0);
Amber77	0:67bd1a7efc59	1418	// break;
Amber77	0:67bd1a7efc59	1419	// }
Amber77	0:67bd1a7efc59	1420	}
Amber77	0:67bd1a7efc59	1421	fclose(fp);
Amber77	0:67bd1a7efc59	1422	}
Amber77	0:67bd1a7efc59	1423

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	30 Aug 2017
Imports:	1
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	5
Followers:	1

main.cpp@0:67bd1a7efc59, 2017-08-30 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning