Skip to content

Commit

Permalink
Merge branch 'main' of https://github.com/ErBWs/CH32-Wireless-Charge
Browse files Browse the repository at this point in the history
  • Loading branch information
@ErBWs
ErBWs committed Apr 14, 2024
2 parents ae7b5bb + 30db52f commit 44c9930
Show file tree
Hide file tree
Showing 94 changed files with 104,236 additions and 84,714 deletions.
102 changes: 57 additions & 45 deletions 2.Firmware/1.CH32V307VCT6/project/code/ctrl.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,16 @@ float dynamic_zero = 0;
extern double X0, Y0;
gps_report_t gpsReport;
bool servo_forbid = false;
float dynamic_gain = 0.008f;
float dynamic_gain = 0.05f;
float normal_dynamic_gain = 0.03f;
float turn_dynamic_gain = 0.55f;

void taskTimAllInit(void) {
pit_ms_init(MAIN_PIT, 2);
pit_ms_init(BEEP_AND_KEY_PIT, 10);
interrupt_set_priority(TIM1_UP_IRQn, (1 << 5) | 1);
interrupt_set_priority(TIM3_IRQn, (2 << 5) | 2);
servo_input_duty = SERVO_MID;
}

void IMUGetCalFun(void) {
Expand Down Expand Up @@ -74,24 +77,32 @@ void IMUGetCalFun(void) {
}

#define USE_BLUE_TOOTH 0
uint16 servo_input_duty = SERVO_MID;
void ServoControl(void) {
uint16 servo_input_duty;
float servo_dither_factor = -0.4f;
bool anti_dither_flag = false;
void ServoControl(int16 encode_val) {
static uint8 counts = 0;
static bool turn_flag = false;
counts++;
#if USE_BLUE_TOOTH == 1
pwm_set_duty(SERVO_PIN,GetServoDuty(dirPid.target[NOW]));
#else

if (stagger_flag == 1 || servo_forbid == true || Bike_Start != 1)return;
// if (stagger_flag == 1 || servo_forbid == true || Bike_Start != 1)return;
// gps_use.delta = gps_use.delta * 0.3 + last_angle * 0.7;
PID_Calculate(&dirPid, 0, (float) gps_use.delta);//纯P

dynamic_zero = (float) (servo_input_duty - SERVO_MID) * dynamic_gain;
dynamic_zero = (float) (servo_input_duty - SERVO_MID)*(float)abs(encode_val) * dynamic_gain/100;
dynamic_zero = LIMIT(dynamic_zero,-8,8);
if(fabsf(dynamic_zero)<0.7)dynamic_zero=0;
// BlueToothPrintf("%f\r\n",dynamic_zero);

if(anti_dither_flag){
dirPid.pos_out += ((float)imu660ra_gyro_z/100 * servo_dither_factor);
}
uint16 duty_temp = GetServoDuty(dirPid.pos_out);
if (abs(duty_temp - servo_input_duty) > fabs(GetServoDuty(2) - SERVO_MID))
turn_flag = true;
if (abs(duty_temp - servo_input_duty) > fabs(GetServoDuty(2) - SERVO_MID));
// turn_flag = true;
if (turn_flag == true) {
if (counts % 3 == 0) {
if (abs(duty_temp - servo_input_duty) > fabs(GetServoDuty(2) - SERVO_MID)) {
Expand All @@ -105,7 +116,7 @@ void ServoControl(void) {
} else {
servo_input_duty = duty_temp;
}
servo_input_duty = LIMIT(servo_input_duty,GetServoDuty(-10), GetServoDuty(10));
servo_input_duty = LIMIT(servo_input_duty,GetServoDuty(-SERVO_MAX_ANGLE), GetServoDuty(SERVO_MAX_ANGLE));


// if(servo_sport_update_flag==0)
Expand All @@ -121,52 +132,54 @@ uint32_t back_inter_distance = 0;
uint8 back_maintain_flag = 1;
int16_t back_wheel_encode = 0;

void BackMotoControl(void) {
int16 BackMotoControl(void) {
static uint8 beg_state = 0, pitch_state = 0;
static uint8 counts = 0;
if (++counts < 20)return;
counts = 0;
static int16 last_encode_val = 0;
if (stagger_flag == 1 || Bike_Start != 1) {
pidClear(&backSpdPid);
motoDutySet(MOTOR_BACK_PIN, 0);
return;
return last_encode_val;
}
if (++counts < 20)return last_encode_val;
counts = 0;

back_wheel_encode = encoder_get_count(ENCODER_BACK_WHEEL_TIM);
last_encode_val = back_wheel_encode = encoder_get_count(ENCODER_BACK_WHEEL_TIM);
encoder_clear_count(ENCODER_BACK_WHEEL_TIM);
back_inter_distance += myABS(back_wheel_encode);

PID_Calculate(&backSpdPid, backSpdPid.target[NOW], (float) back_wheel_encode);//速度环PID
switch (pitch_state) {
case 0:
if (imu_data.pit > 9) {
servo_forbid = true;
pwm_set_duty(SERVO_PIN, SERVO_MID);
backSpdPid.target[NOW] = 100;
backSpdPid.Ki = 0;
beepTime = 400;
pitch_state = 1;
}
break;
case 1:
if (imu_data.pit < 1) {
backSpdPid.target[NOW] = fast_velocity;
// backSpdPid.pos_out -= backSpdPid.iout;//消除积分作用
backSpdPid.iout = 0;
back_inter_distance = 0;
beepTime = 400;
pitch_state = 2;
}
break;
case 2:
if (back_inter_distance > 250) {
servo_forbid = false;
backSpdPid.Ki = 3;
beepTime = 400;
pitch_state = 0;
}
}
// switch (pitch_state) {
// case 0:
// if (imu_data.pit > 9) {
// servo_forbid = true;
// pwm_set_duty(SERVO_PIN, SERVO_MID);
// backSpdPid.target[NOW] = 100;
// backSpdPid.Ki = 0;
// beepTime = 400;
// pitch_state = 1;
// }
// break;
// case 1:
// if (imu_data.pit < 1) {
// backSpdPid.target[NOW] = fast_velocity;
//// backSpdPid.pos_out -= backSpdPid.iout;//消除积分作用
// backSpdPid.iout = 0;
// back_inter_distance = 0;
// beepTime = 400;
// pitch_state = 2;
// }
// break;
// case 2:
// if (back_inter_distance > 250) {
// servo_forbid = false;
// backSpdPid.Ki = 3;
// beepTime = 400;
// pitch_state = 0;
// }
// }
motoDutySet(MOTOR_BACK_PIN, (int32) -backSpdPid.pos_out);
return last_encode_val;
}

uint8 stagger_flag = 1;
Expand Down Expand Up @@ -194,14 +207,13 @@ void FlyWheelControl(void) {
}
if (counts % 2 == 0)//4
{
PID_Calculate(&flyAnglePid, (flySpdPid.pos_out < 0 ? -sqrtf(-flySpdPid.pos_out) : sqrtf(flySpdPid.pos_out)) +
ANGLE_STATIC_BIAS + dynamic_zero, imu_data.rol);//角度环PD
PID_Calculate(&flyAnglePid, flySpdPid.pos_out + ANGLE_STATIC_BIAS + dynamic_zero, imu_data.rol);//角度环PD
// printf("A%f\r\n",imu_data.rol);
// BlueToothPrintf("%f\r\n",imu_data.rol);
}
PID_Calculate(&flyAngleSpdPid, flyAnglePid.pos_out, temp_x);//角速度环PI// printf("B%f\r\n",temp_x);

if (abs(imu_data.rol) > 35) {
if (abs(imu_data.rol) > 30) {
stagger_flag = 1;
motoDutySet(MOTOR_FLY_PIN, 0);
return;
Expand Down
8 changes: 6 additions & 2 deletions 2.Firmware/1.CH32V307VCT6/project/code/ctrl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,16 +9,20 @@
extern uint32_t back_inter_distance;
extern uint8 stagger_flag;
extern float dynamic_gain;
extern float normal_dynamic_gain;
extern float turn_dynamic_gain;
extern bool servo_forbid;
extern uint16 servo_input_duty;
extern bool anti_dither_flag;
extern float servo_dither_factor;
extern paramType ANGLE_STATIC_BIAS;
extern float temp_x;
extern int16_t fly_wheel_encode;
extern uint32_t myTimeStamp;
void IMUGetCalFun(void);
void ServoControl(void);
void ServoControl(int16 encode_val);
void taskTimAllInit(void);
void BackMotoControl(void);
int16 BackMotoControl(void);
void FlyWheelControl(void);
void UpdateControl(void);
#endif
20 changes: 16 additions & 4 deletions 2.Firmware/1.CH32V307VCT6/project/code/easy_ui_user_app.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ extern gps_report_t gpsReport;
EasyUIPage_t pageWelcome, pageMain, pagePreset, pageFlyWheelPID, pageDirPID, pageBackMotorPID, pageThreshold, pageCam, pagePoints, pageNormalPoints, pagePathGenerate,pageBasePoints,pageConePoints,pagePilePoints,pageSetting, pageAbout, pageVoltage, pageGenerateCone,pageGeneratePile;

// Items
EasyUIItem_t titleMain, itemRun, itemPreset, itemSpdPID, itemDirPID, itemBackMotor, itemSlowVel, itemFastVel, itemTurnVel, itemSlowServo, itemFastServo, itemTurnServo, itemThreshold, itemCam, itemGPS,itemSetKgain,itemSetYawBias,itemSetStaticAngle,itemSetting,itemGenCone,itemGenPile;
EasyUIItem_t titleMain, itemRun, itemPreset, itemSpdPID, itemDirPID, itemBackMotor, itemNorDynaGain, itemTurnDynaGain,itemSlowVel, itemFastVel, itemTurnVel, itemRampVel, itemSlowServo, itemFastServo, itemTurnServo, itemEncode1, itemEncode2, itemThreshold, itemCam, itemGPS,itemSetKgain,itemSetYawBias,itemSetStaticAngle, itemSetServoCalibration, itemSetServoDitherFactor, itemSetting,itemGenCone,itemGenPile;
EasyUIItem_t titleGPS, itemBasePoints,itemNormalPoints,itemConePoints,itemPilePoints,itemPathGenerate, itemSavePoints, itemReadPoints,itemCNX,itemCNY,itemSSD,itemCYF,itemEGFN,itemSCY,itemSMC,itemSetIndex,itemSRY,itemSetConeCounts,itemSetConeTotalDis,itemSetConeHorizonDis,itemSetConeDir,itemSetPileRadius,itemSetPileDir;
EasyUIItem_t titleSpdPID, itemSpdKp, itemSpdKi, itemSpdKd, itemAngKp, itemAngKi, itemAngKd, itemAngSpdKp, itemAngSpdKi, itemAngSpdKd, KpitemSpdTarget, itemSpdInMax, itemSpdErrMax, itemSpdErrMin;
EasyUIItem_t titleDirPID, itemDirKp, itemDirKi, itemDirKd, itemDirInMax, itemDirErrMax, itemDirErrMin;
Expand All @@ -32,6 +32,7 @@ void EventMainLoop(EasyUIItem_t *item)
cone_handler_index=0;
cone_handler_flag = false;
dirPid.Kp = fast_servo_kp;
dynamic_gain = normal_dynamic_gain;
motoDutySet(SERVO_PIN,SERVO_MID);
servo_input_duty = SERVO_MID;
if(!GlobalGraph.is_init ||!GlobalGraph.B_constructor->is_interpolated)
Expand All @@ -41,7 +42,7 @@ void EventMainLoop(EasyUIItem_t *item)
EasyUIBackgroundBlur();
return;
}
stanleyControllerInit(&Global_stanleyController,(float)0.3,(float)0.05,&Global_yaw,&Global_v_now,&Global_current_node);
stanleyControllerInit(&Global_stanleyController,(float)Global_k_gain,(float)0.05,&Global_yaw,&Global_v_now,&Global_current_node);
status|=stanleyBuffLink(&Global_stanleyController,Global_pd_array,NULL,GlobalGraph.total);
status|=stanley_GraphRegister(&GlobalGraph,&Global_stanleyController);
status|=GraphNode_Diff(&GlobalGraph);
Expand Down Expand Up @@ -76,6 +77,7 @@ void EventMainLoop(EasyUIItem_t *item)
}
pidClear(&backSpdPid);
backSpdPid.target[NOW]=fast_velocity;
anti_dither_flag = true;
while(1)
{
static uint16 temp=4000;
Expand Down Expand Up @@ -103,12 +105,14 @@ void EventMainLoop(EasyUIItem_t *item)
functionIsRunning = false;
beepTime = 1500;
Bike_Start = 0;
anti_dither_flag = false;
break;
}
}
if (opnExit)
{
motoDutySet(SERVO_PIN,SERVO_MID);
anti_dither_flag = false;
Bike_Start = 0;
opnExit = false;
functionIsRunning = false;
Expand Down Expand Up @@ -190,7 +194,7 @@ void EventPathGenerate(EasyUIItem_t *item)
{
GraphInit(&GlobalGraph, GlobalGraph_NodeBuffer, &GlobalBase_GPS_data, PATH_TOTAL_COUNTS);
status|=B_ConstructorInit(&Global_B_Constructor, gps_use.point_count, B_ORDER);
status|=B_ConstructorBuffLink(&Global_B_Constructor, GlobalNodeVector, GlobalNipFactorVector, GlobalRefNodeList);
status|=B_ConstructorBuffLink(&Global_B_Constructor, GlobalNodeVector, GlobalRefNodeList);
status|=B_GraphRegister(&GlobalGraph, &Global_B_Constructor);
uint8_t GraphReferNodeConvertInput(nodeGraph_typedef *graph, gps_st *gps_set, uint16_t counts);
GraphReferNodeConvertInput(&GlobalGraph,gps_data_array,gps_use.point_count);
Expand Down Expand Up @@ -510,18 +514,26 @@ void MenuInit()
EasyUIAddItem(&pageMain, &titleMain, "[Main]", ITEM_PAGE_DESCRIPTION);
EasyUIAddItem(&pageMain, &itemRun, "Run", ITEM_MESSAGE, "Running...", EventMainLoop);
EasyUIAddItem(&pageMain, &itemGPS, "GPS Points", ITEM_JUMP_PAGE, pagePoints.id);
EasyUIAddItem(&pageMain, &itemSetKgain, "Set K-gain", ITEM_CHANGE_VALUE, &Global_stanleyController.k_gain, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemSetKgain, "Set K-gain", ITEM_CHANGE_VALUE, &Global_k_gain, EasyUIEventChangeFloat);
// EasyUIAddItem(&pageMain, &itemSetYawBias, "Set Yaw-Bias", ITEM_CHANGE_VALUE, &yaw_angle_bias, EasyUIEventChangeFloatForYaw);
EasyUIAddItem(&pageMain, &itemSetStaticAngle, "Set Static-Angle", ITEM_CHANGE_VALUE, &ANGLE_STATIC_BIAS, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemSetServoCalibration, "Set Servo-Cali", ITEM_CHANGE_VALUE, &global_servo_calibration, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemSetServoDitherFactor, "Set Dither-Factor", ITEM_CHANGE_VALUE, &servo_dither_factor, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemSpdPID, "Fly-Wheel PID", ITEM_JUMP_PAGE, pageFlyWheelPID.id);
EasyUIAddItem(&pageMain, &itemDirPID, "Direction PID", ITEM_JUMP_PAGE, pageDirPID.id);
EasyUIAddItem(&pageMain, &itemBackMotor, "BackMotor PID", ITEM_JUMP_PAGE, pageBackMotorPID.id);
EasyUIAddItem(&pageMain, &itemNorDynaGain, "Normal Dynamic Gain", ITEM_CHANGE_VALUE, &normal_dynamic_gain, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemTurnDynaGain, "Turn Dynamic Gain", ITEM_CHANGE_VALUE, &turn_dynamic_gain, EasyUIEventChangeFloat);

EasyUIAddItem(&pageMain, &itemSlowVel, "Set Slow Velocity", ITEM_CHANGE_VALUE, &slow_velocity, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemFastVel, "Set Fast Velocity", ITEM_CHANGE_VALUE, &fast_velocity, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemTurnVel, "Set Turn Velocity", ITEM_CHANGE_VALUE, &turn_velocity, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemRampVel, "Set Ramp Velocity", ITEM_CHANGE_VALUE, &ramp_velocity, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemSlowServo, "Set Slow Servo", ITEM_CHANGE_VALUE, &slow_servo_kp, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemFastServo, "Set Fast Servo", ITEM_CHANGE_VALUE, &fast_servo_kp, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemTurnServo, "Set Turn Servo", ITEM_CHANGE_VALUE, &turn_servo_kp, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemEncode1, "Set Encode-1", ITEM_CHANGE_VALUE, &Global_encode1, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemEncode2, "Set Encode-2", ITEM_CHANGE_VALUE, &Global_encode2, EasyUIEventChangeFloat);
EasyUIAddItem(&pageMain, &itemSetting, "Settings", ITEM_JUMP_PAGE, pageSetting.id);

// Page GPS points
Expand Down
20 changes: 9 additions & 11 deletions 2.Firmware/1.CH32V307VCT6/project/code/graph_mapping.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,18 +78,16 @@ uint8_t B_ConstructorInit(B_Constructor_typedef *constructor,uint8_t ref_counts,
constructor->is_init = 1;
return 0;
}
uint8_t B_ConstructorBuffLink(B_Constructor_typedef *constructor, double *NodeVector, double *NipFactorVector, nodeLink_typedef refNodeList)
uint8_t B_ConstructorBuffLink(B_Constructor_typedef *constructor, double *NodeVector, nodeLink_typedef refNodeList)
{
if(!constructor->is_init)
{
printf("constructor may not be initialized!");
return 1;
}
memset(NodeVector,0, sizeof(double)*(constructor->B_m + 1));
memset(NipFactorVector,0,sizeof(double)*(constructor->B_n + 1));
memset(refNodeList,0, sizeof(node_typedef)*(constructor->B_n + 1));
constructor->NodeVector = NodeVector;
constructor->NipFactorVector = NipFactorVector;
constructor->refNodeList = refNodeList;
constructor->is_link = 1;
return 0;
Expand Down Expand Up @@ -225,9 +223,9 @@ uint8_t GraphReferNodeConvertInput(nodeGraph_typedef *graph, gps_st *gps_set, ui
return 1;
}
nodeLink_typedef refNodeList;
gpsData_typedef base_gps_data;
// gpsData_typedef base_gps_data;
refNodeList = graph->B_constructor->refNodeList;
base_gps_data = *graph->base_gps_data;
// base_gps_data = *graph->base_gps_data;
// double dx_lat,dy_lon;
// latlonTodxdy(base_gps_data.latitude,&dx_lat,&dy_lon);
// refNodeList[0].X = ANGLE_TO_RAD(gps_set[0].latitude - base_gps_data.latitude)*dx_lat;
Expand Down Expand Up @@ -266,7 +264,7 @@ uint8_t GraphPathGenerate(nodeGraph_typedef *graph)
printf("graph may not be initialized or has no B_constructor!");
return 1;
}
double step,u;
double step,u,NipFactor;
B_Constructor_typedef *constructor;
step = (double)(1.0/(graph->total-1));
constructor = graph->B_constructor;
Expand All @@ -277,12 +275,12 @@ uint8_t GraphPathGenerate(nodeGraph_typedef *graph)
u = k * step;
for(int i=0;i<constructor->B_n+1;i++)
{
if(u>=i*step&&u<i+constructor->B_p+1)//avoid meaningless iterations
constructor->NipFactorVector[i] = BaseIterateFunc(i,constructor->B_p,u,constructor->NodeVector);
if(u>=i*step&&u<(i+constructor->B_p+1)*step)//avoid meaningless iterations
NipFactor = BaseIterateFunc(i,constructor->B_p,u,constructor->NodeVector);
else
constructor->NipFactorVector[i] = 0;
graph->nodeBuff[k].X += constructor->NipFactorVector[i] * constructor->refNodeList[i].X;
graph->nodeBuff[k].Y += constructor->NipFactorVector[i] * constructor->refNodeList[i].Y;
NipFactor = 0;
graph->nodeBuff[k].X += NipFactor * constructor->refNodeList[i].X;
graph->nodeBuff[k].Y += NipFactor * constructor->refNodeList[i].Y;
}
}
constructor->is_interpolated = 1;
Expand Down
7 changes: 3 additions & 4 deletions 2.Firmware/1.CH32V307VCT6/project/code/graph_mapping.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@

typedef struct
{
double latitude; // 当前目标点经度
double longitude; // 当前目标点纬度
double latitude; // ��ǰĿ��㾭��
double longitude; // ��ǰĿ���γ��
}gpsData_typedef,*gpsDataLink_typedef;

typedef struct
Expand All @@ -50,7 +50,6 @@ typedef struct
uint8_t is_link : 1;
uint8_t is_interpolated : 1;
double *NodeVector;
double *NipFactorVector;
nodeLink_typedef refNodeList;
}B_Constructor_typedef,*B_Constructor_Link_typedef;
typedef struct {
Expand Down Expand Up @@ -99,7 +98,7 @@ extern stanleyController_typedef Global_stanleyController;
void latlonTodxdy(double lat, double *dx_dlat, double *dy_dlon);
void WGS_84_ConvertToXY(double base_latitude, double base_longitude, gpsDataLink_typedef gpsDATA, nodeLink_typedef nodesDATA, uint16_t counts);
uint8_t B_ConstructorInit(B_Constructor_typedef *constructor,uint8_t ref_counts,uint8_t order);
uint8_t B_ConstructorBuffLink(B_Constructor_typedef *constructor, double *NodeVector, double *NipFactorVector, nodeLink_typedef refNodeList);
uint8_t B_ConstructorBuffLink(B_Constructor_typedef *constructor, double *NodeVector, nodeLink_typedef refNodeList);
uint8_t B_GraphRegister(nodeGraph_typedef *graph, B_Constructor_typedef *constructor);
void stanleyControllerInit(stanleyController_typedef *controller, float k_gain, float L, float *yaw, float *v_now,node_typedef *current_node);
uint8_t stanleyBuffLink(stanleyController_typedef *controller, float *pd_array,float *pdd_array,uint16_t point_total);
Expand Down
Loading

0 comments on commit 44c9930

Please sign in to comment.