btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat 更新角度闭环相关功能

Browse Source
This commit is contained in:
井明江
2024-01-16 20:03:21 +08:00
parent 16ead48bba
commit 3bc5e3cf93
19 changed files with 908 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
3rd-lib/PID-Library/LICENSE Normal file
View File

@@ -0,0 +1,21 @@
MIT License
Copyright (c) 2021 Majid Derhambakhsh
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

164
3rd-lib/PID-Library/README.md Normal file
View File

@@ -0,0 +1,164 @@
![Banner](Banner.png)
# PID-Library (C Version)
PID Controller library for ARM Cortex M (STM32)
> #### Download Arduino Library : [Arduino-PID-Library](https://github.com/br3ttb/Arduino-PID-Library)
## Release
- #### Version : 1.0.0
- #### Type : Embedded Software.
- #### Support :
- ARM STM32 series
- #### Program Language : C/C++
- #### Properties :
- #### Changes :
- #### Required Library/Driver :
## Overview
### Initialization and de-initialization functions:
```c++
void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection);
void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection);
```
### Operation functions:
```c++
/* ::::::::::: Computing ::::::::::: */
uint8_t PID_Compute(PID_TypeDef *uPID);
/* ::::::::::: PID Mode :::::::::::: */
void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode);
PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID);
/* :::::::::: PID Limits ::::::::::: */
void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max);
/* :::::::::: PID Tunings :::::::::: */
void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd);
void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn);
/* ::::::::: PID Direction ::::::::: */
void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction);
PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID);
/* ::::::::: PID Sampling :::::::::: */
void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime);
/* ::::::: Get Tunings Param ::::::: */
float PID_GetKp(PID_TypeDef *uPID);
float PID_GetKi(PID_TypeDef *uPID);
float PID_GetKd(PID_TypeDef *uPID);
```
### Macros:
```diff
non
```
## Guide
#### This library can be used as follows:
#### 1. Add pid.h header
#### 2. Create PID struct and initialize it, for example:
* Initializer:
```c++
PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection);
```
* Parameters:
* uPID : Pointer to pid struct
* Input : The variable we're trying to control (float)
* Output : The variable that will be adjusted by the pid (float)
* Setpoint : The value we want to Input to maintain (float)
* Kp,Ki,Kd : Tuning Parameters. these affect how the pid will change the output (float>=0)
* POn : Either P_ON_E (Default) or P_ON_M. Allows Proportional on Measurement to be specified.
* ControllerDirection : Either DIRECT or REVERSE. determines which direction the output will move when faced with a given error. DIRECT is most common
* Example:
```c++
PID_TypeDef TPID;
float Temp, PIDOut, TempSetpoint;
PID(&TPID, &Temp, &PIDOut, &TempSetpoint, 2, 5, 1, _PID_P_ON_E, _PID_CD_DIRECT);
```
#### 3. Set 'mode', 'sample time' and 'output limit', for example:
* Functions:
```c++
void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode);
void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max);
void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime);
```
* Parameters:
* uPID : Pointer to pid struct
* Mode : _PID_MODE_AUTOMATIC or _PID_MODE_MANUAL
* Min : Low end of the range. must be < max (float)
* Max : High end of the range. must be > min (float)
* NewSampleTime : How often, in milliseconds, the PID will be evaluated. (int>0)
* Example:
```c++
PID_SetMode(&TPID, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&TPID, 500);
PID_SetOutputLimits(&TPID, 1, 100);
```
#### 4. Using Compute function, for example:
```c++
PID_Compute(&TPID);
```
## Examples
#### Example 1: PID Compute for temperature
```c++
#include "main.h"
#include "pid.h"
PID_TypeDef TPID;
char OutBuf[50];
float Temp, PIDOut, TempSetpoint;
int main(void)
{
HW_Init();
PID(&TPID, &Temp, &PIDOut, &TempSetpoint, 2, 5, 1, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&TPID, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&TPID, 500);
PID_SetOutputLimits(&TPID, 1, 100);
while (1)
{
Temp = GetTemp();
PID_Compute(&TPID);
sprintf(OutBuf, "Temp%3.2f : %u\n", Temp, (uint16_t)PIDOut);
UART_Transmit((uint8_t *)OutBuf, strlen(OutBuf));
Delay_ms(500);
}
}
```
## Tests performed:
- [x] Run on STM32 Fx cores
## Developers:
- ### Majid Derhambakhsh

293
3rd-lib/PID-Library/pid.c Normal file
View File

@@ -0,0 +1,293 @@
 /*
------------------------------------------------------------------------------
~ File : pid.c
~ Author : Majid Derhambakhsh
~ Version: V1.0.0
~ Created: 02/11/2021 03:43:00 AM
~ Brief :
~ Support:
E-Mail : Majid.do16@gmail.com (subject : Embedded Library Support)
Github : https://github.com/Majid-Derhambakhsh
------------------------------------------------------------------------------
~ Description:
~ Attention :
~ Changes :
------------------------------------------------------------------------------
*/
#include "pid.h"
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ~~~~~~~~~~~~~~~~~ Initialize ~~~~~~~~~~~~~~~~ */
void PID_Init(PID_TypeDef *uPID)
{
/* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */
uPID->OutputSum = *uPID->MyOutput;
uPID->LastInput = *uPID->MyInput;
if (uPID->OutputSum > uPID->OutMax)
{
uPID->OutputSum = uPID->OutMax;
}
else if (uPID->OutputSum < uPID->OutMin)
{
uPID->OutputSum = uPID->OutMin;
}
}
void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection)
{
/* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */
uPID->MyOutput = Output;
uPID->MyInput = Input;
uPID->MySetpoint = Setpoint;
uPID->InAuto = (PIDMode_TypeDef)_FALSE;
PID_SetOutputLimits(uPID, 0, _PID_8BIT_PWM_MAX);
uPID->SampleTime = _PID_SAMPLE_TIME_MS_DEF; /* default Controller Sample Time is 0.1 seconds */
PID_SetControllerDirection(uPID, ControllerDirection);
PID_SetTunings2(uPID, Kp, Ki, Kd, POn);
}
void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection)
{
PID(uPID, Input, Output, Setpoint, Kp, Ki, Kd, _PID_P_ON_E, ControllerDirection);
}
/* ~~~~~~~~~~~~~~~~~ Computing ~~~~~~~~~~~~~~~~~ */
uint8_t PID_Compute(PID_TypeDef *uPID)
{
float input=0;
float error=0;
float dInput=0;
float output=0;
/* ~~~~~~~~~~ Check PID mode ~~~~~~~~~~ */
if (!uPID->InAuto)
{
return _FALSE;
}
/* ..... Compute all the working error variables ..... */
input = *uPID->MyInput;
error = *uPID->MySetpoint - input;
dInput = (input - uPID->LastInput);
uPID->OutputSum += (uPID->Ki * error);
/* ..... Add Proportional on Measurement, if P_ON_M is specified ..... */
if (!uPID->POnE)
{
uPID->OutputSum -= uPID->Kp * dInput;
}
if (uPID->OutputSum > uPID->OutMax)
{
uPID->OutputSum = uPID->OutMax;
}
else if (uPID->OutputSum < uPID->OutMin)
{
uPID->OutputSum = uPID->OutMin;
}
else { }
/* ..... Add Proportional on Error, if P_ON_E is specified ..... */
if (uPID->POnE)
{
output = uPID->Kp * error;
}
else
{
output = 0;
}
/* ..... Compute Rest of PID Output ..... */
output += uPID->OutputSum - uPID->Kd * dInput;
if (output > uPID->OutMax)
{
output = uPID->OutMax;
}
else if (output < uPID->OutMin)
{
output = uPID->OutMin;
}
else { }
*uPID->MyOutput = output;
/* ..... Remember some variables for next time ..... */
uPID->LastInput = input;
return _TRUE;
}
/* ~~~~~~~~~~~~~~~~~ PID Mode ~~~~~~~~~~~~~~~~~~ */
void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode)
{
uint8_t newAuto = (Mode == _PID_MODE_AUTOMATIC);
/* ~~~~~~~~~~ Initialize the PID ~~~~~~~~~~ */
if (newAuto && !uPID->InAuto)
{
PID_Init(uPID);
}
uPID->InAuto = (PIDMode_TypeDef)newAuto;
}
PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID)
{
return uPID->InAuto ? _PID_MODE_AUTOMATIC : _PID_MODE_MANUAL;
}
/* ~~~~~~~~~~~~~~~~ PID Limits ~~~~~~~~~~~~~~~~~ */
void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max)
{
/* ~~~~~~~~~~ Check value ~~~~~~~~~~ */
if (Min >= Max)
{
return;
}
uPID->OutMin = Min;
uPID->OutMax = Max;
/* ~~~~~~~~~~ Check PID Mode ~~~~~~~~~~ */
if (uPID->InAuto)
{
/* ..... Check out value ..... */
if (*uPID->MyOutput > uPID->OutMax)
{
*uPID->MyOutput = uPID->OutMax;
}
else if (*uPID->MyOutput < uPID->OutMin)
{
*uPID->MyOutput = uPID->OutMin;
}
else { }
/* ..... Check out value ..... */
if (uPID->OutputSum > uPID->OutMax)
{
uPID->OutputSum = uPID->OutMax;
}
else if (uPID->OutputSum < uPID->OutMin)
{
uPID->OutputSum = uPID->OutMin;
}
else { }
}
}
/* ~~~~~~~~~~~~~~~~ PID Tunings ~~~~~~~~~~~~~~~~ */
void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd)
{
PID_SetTunings2(uPID, Kp, Ki, Kd, uPID->POn);
}
void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn)
{
float SampleTimeInSec;
/* ~~~~~~~~~~ Check value ~~~~~~~~~~ */
if (Kp < 0 || Ki < 0 || Kd < 0)
{
return;
}
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
uPID->POn = POn;
uPID->POnE = (PIDPON_TypeDef)(POn == _PID_P_ON_E);
uPID->DispKp = Kp;
uPID->DispKi = Ki;
uPID->DispKd = Kd;
/* ~~~~~~~~~ Calculate time ~~~~~~~~ */
SampleTimeInSec = ((float)uPID->SampleTime) / 1000;
uPID->Kp = Kp;
uPID->Ki = Ki * SampleTimeInSec;
uPID->Kd = Kd / SampleTimeInSec;
/* ~~~~~~~~ Check direction ~~~~~~~~ */
if (uPID->ControllerDirection == _PID_CD_REVERSE)
{
uPID->Kp = (0 - uPID->Kp);
uPID->Ki = (0 - uPID->Ki);
uPID->Kd = (0 - uPID->Kd);
}
}
/* ~~~~~~~~~~~~~~~ PID Direction ~~~~~~~~~~~~~~~ */
void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction)
{
/* ~~~~~~~~~~ Check parameters ~~~~~~~~~~ */
if ((uPID->InAuto) && (Direction !=uPID->ControllerDirection))
{
uPID->Kp = (0 - uPID->Kp);
uPID->Ki = (0 - uPID->Ki);
uPID->Kd = (0 - uPID->Kd);
}
uPID->ControllerDirection = Direction;
}
PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID)
{
return uPID->ControllerDirection;
}
/* ~~~~~~~~~~~~~~~ PID Sampling ~~~~~~~~~~~~~~~~ */
void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime)
{
float ratio;
/* ~~~~~~~~~~ Check value ~~~~~~~~~~ */
if (NewSampleTime > 0)
{
ratio = (float)NewSampleTime / (float)uPID->SampleTime;
uPID->Ki *= ratio;
uPID->Kd /= ratio;
uPID->SampleTime = (uint32_t)NewSampleTime;
}
}
/* ~~~~~~~~~~~~~ Get Tunings Param ~~~~~~~~~~~~~ */
float PID_GetKp(PID_TypeDef *uPID)
{
return uPID->DispKp;
}
float PID_GetKi(PID_TypeDef *uPID)
{
return uPID->DispKi;
}
float PID_GetKd(PID_TypeDef *uPID)
{
return uPID->DispKd;
}

176
3rd-lib/PID-Library/pid.h Normal file
View File

@@ -0,0 +1,176 @@
/*
------------------------------------------------------------------------------
~ File : pid.h
~ Author : Majid Derhambakhsh
~ Version: V1.0.0
~ Created: 02/11/2021 03:43:00 AM
~ Brief :
~ Support:
E-Mail : Majid.do16@gmail.com (subject : Embedded Library Support)
Github : https://github.com/Majid-Derhambakhsh
------------------------------------------------------------------------------
~ Description:
~ Attention :
~ Changes :
------------------------------------------------------------------------------
*/
#ifndef __PID_H_
#define __PID_H_
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Include ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
#include <stdint.h>
#include <string.h>
#include "zf_common_headfile.h"
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Defines ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ------------------------ Library ------------------------ */
#define _PID_LIBRARY_VERSION 1.0.0
/* ------------------------ Public ------------------------- */
#define _PID_8BIT_PWM_MAX UINT8_MAX
#define _PID_SAMPLE_TIME_MS_DEF 100
#ifndef _FALSE
#define _FALSE 0
#endif
#ifndef _TRUE
#define _TRUE 1
#endif
/* ---------------------- By compiler ---------------------- */
#ifndef GetTime
/* ---------------------- By compiler ---------------------- */
#ifdef __CODEVISIONAVR__ /* Check compiler */
#define GetTime() 0
/* ------------------------------------------------------------------ */
#elif defined(__GNUC__) /* Check compiler */
#define GetTime() 0
/* ------------------------------------------------------------------ */
#else
#endif /* __CODEVISIONAVR__ */
/* ------------------------------------------------------------------ */
#endif
/* --------------------------------------------------------- */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Types ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* PID Mode */
typedef enum
{
_PID_MODE_MANUAL = 0,
_PID_MODE_AUTOMATIC = 1
}PIDMode_TypeDef;
/* PID P On x */
typedef enum
{
_PID_P_ON_M = 0, /* Proportional on Measurement */
_PID_P_ON_E = 1
}PIDPON_TypeDef;
/* PID Control direction */
typedef enum
{
_PID_CD_DIRECT = 0,
_PID_CD_REVERSE = 1
}PIDCD_TypeDef;
/* PID Structure */
typedef struct
{
PIDPON_TypeDef POnE;
PIDMode_TypeDef InAuto;
PIDPON_TypeDef POn;
PIDCD_TypeDef ControllerDirection;
uint32_t LastTime;
uint32_t SampleTime;
float DispKp;
float DispKi;
float DispKd;
float Kp;
float Ki;
float Kd;
float *MyInput;
float *MyOutput;
float *MySetpoint;
float OutputSum;
float LastInput;
float OutMin;
float OutMax;
}PID_TypeDef;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Enum ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Struct ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Class ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Prototype ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* :::::::::::::: Init ::::::::::::: */
void PID_Init(PID_TypeDef *uPID);
void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection);
void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection);
/* ::::::::::: Computing ::::::::::: */
uint8_t PID_Compute(PID_TypeDef *uPID);
/* ::::::::::: PID Mode :::::::::::: */
void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode);
PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID);
/* :::::::::: PID Limits ::::::::::: */
void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max);
/* :::::::::: PID Tunings :::::::::: */
void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd);
void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn);
/* ::::::::: PID Direction ::::::::: */
void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction);
PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID);
/* ::::::::: PID Sampling :::::::::: */
void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime);
/* ::::::: Get Tunings Param ::::::: */
float PID_GetKp(PID_TypeDef *uPID);
float PID_GetKi(PID_TypeDef *uPID);
float PID_GetKd(PID_TypeDef *uPID);
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ End of the program ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
#endif /* __PID_H_ */

6
app/by_buzzer.c
View File

@@ -29,14 +29,14 @@ void queue_pop_element(void)
void queue_pop_read(void) void queue_pop_read(void)
{ {
while (queue_long != 0) { while (queue_long != 0) {
pwm_init(TIM8_PWM_MAP0_CH1_C6, a[0], 5000); pwm_init(BUZZER_PIN, a[0], 5000);
queue_pop_element(); queue_pop_element();
system_delay_ms(100); system_delay_ms(100);
pwm_set_duty(TIM8_PWM_MAP0_CH1_C6, 0); pwm_set_duty(BUZZER_PIN, 0);
} }
} }
void by_buzzer_init(void) void by_buzzer_init(void)
{ {
pwm_init(TIM8_PWM_MAP0_CH1_C6, 2000, 0); pwm_init(BUZZER_PIN, 2000, 0);
} }

2
app/by_buzzer.h
View File

@@ -10,7 +10,7 @@
#define BY_PRESS_LONG 2500 #define BY_PRESS_LONG 2500
#define BY_FORWARD 1500 #define BY_FORWARD 1500
#define BY_BACKWARD 1800 #define BY_BACKWARD 1800
#define BUZZER_PIN TIM3_PWM_MAP0_CH2_A7
extern void by_buzzer_init(void); extern void by_buzzer_init(void);
extern void queue_init(void); extern void queue_init(void);
extern void queue_add_element(int element); extern void queue_add_element(int element);

60
app/by_fan_control.c
View File

@@ -1,28 +1,58 @@
#include "by_fan_control.h" #include "by_fan_control.h"
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#define Fan_pwm_up1 TIM8_PWM_MAP0_CH1_C6
#define Fan_pwm_up2 TIM8_PWM_MAP0_CH2_C7
#define Fan_pwm_power1 TIM4_PWM_MAP1_CH3_D14
#define Fan_pwm_power2 TIM4_PWM_MAP1_CH4_D15
#define Fan_pwm_power3 TIM4_PWM_MAP1_CH2_D13
#define Fan_pwm_power4 TIM4_PWM_MAP1_CH1_D12
uint32_t myclip(uint32_t x, uint32_t low, uint32_t up)
{
return (x > up ? up : x < low ? low
: x);
}
void by_pwm_init(void) void by_pwm_init(void)
{ {
pwm_init(TIM4_PWM_MAP1_CH1_D12, 10000, 0); // 浮力风扇左 pwm_init(Fan_pwm_up1, 50, 500); // 浮力风扇左
pwm_init(TIM4_PWM_MAP1_CH2_D13, 10000, 0); // 浮力风扇右 pwm_init(Fan_pwm_up2, 50, 500); // 浮力风扇右
pwm_init(TIM4_PWM_MAP1_CH3_D14, 10000, 0); // 动力风扇左 pwm_init(Fan_pwm_power1, 50, 500); // 动力风扇左1
pwm_init(TIM4_PWM_MAP1_CH4_D15, 10000, 0); // 动力风扇右 pwm_init(Fan_pwm_power2, 50, 500); // 动力风扇右1
pwm_init(Fan_pwm_power3, 50, 500); // 动力风扇左2
pwm_init(Fan_pwm_power4, 50, 500); // 动力风扇右2
// system_delay_ms(3000);
// // pwm_init(Fan_pwm_power1, 50, 1000); // 动力风扇左1
// // pwm_init(Fan_pwm_power2, 50, 1000); // 动力风扇右1
// // pwm_init(Fan_pwm_power3, 50, 1000); // 动力风扇左2
// // pwm_init(Fan_pwm_power4, 50, 1000); // 动力风扇右2
// // system_delay_ms(5000);
// pwm_set_duty(Fan_pwm_power1, 600);
// pwm_set_duty(Fan_pwm_power2, 600);
// pwm_set_duty(Fan_pwm_power3, 600);
// pwm_set_duty(Fan_pwm_power4, 600);
// while(1);
} }
void by_pwm_update_duty(uint32_t update_pwm_duty) void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2)
{ {
if (4000UL < update_pwm_duty) { update_pwm_duty1=myclip(update_pwm_duty1, 500, 900);
update_pwm_duty = 4000UL; update_pwm_duty2=myclip(update_pwm_duty2, 500, 900);
} pwm_set_duty(Fan_pwm_up1, update_pwm_duty1);
pwm_set_duty(TIM4_PWM_MAP1_CH1_D12, update_pwm_duty); pwm_set_duty(Fan_pwm_up2, update_pwm_duty2);
pwm_set_duty(TIM4_PWM_MAP1_CH2_D13, update_pwm_duty);
// pwm_set_duty(TIM4_PWM_MAP1_CH3_D14, update_pwm_duty);
// pwm_set_duty(TIM4_PWM_MAP1_CH4_D15, update_pwm_duty);
} }
void by_pwm_power_duty(uint32_t power_pwm_duty_l, uint32_t power_pwm_duty_r) void by_pwm_power_duty(uint32_t power_pwm_duty_l1, uint32_t power_pwm_duty_r1, uint32_t power_pwm_duty_l2, uint32_t power_pwm_duty_r2)
{ {
pwm_set_duty(TIM4_PWM_MAP1_CH3_D14, power_pwm_duty_l);
pwm_set_duty(TIM4_PWM_MAP1_CH4_D15, power_pwm_duty_r); power_pwm_duty_l1=myclip(power_pwm_duty_l1, 500, 900);
power_pwm_duty_r1=myclip(power_pwm_duty_r1, 500, 900);
power_pwm_duty_l2=myclip(power_pwm_duty_l2, 500, 900);
power_pwm_duty_r2=myclip(power_pwm_duty_r2, 500, 900);
pwm_set_duty(Fan_pwm_power1, power_pwm_duty_l1);
pwm_set_duty(Fan_pwm_power2, power_pwm_duty_r1);
pwm_set_duty(Fan_pwm_power3, power_pwm_duty_l2);
pwm_set_duty(Fan_pwm_power4, power_pwm_duty_r2);
} }

4
app/by_fan_control.h
View File

@@ -5,7 +5,7 @@
#include "ch32v30x.h" #include "ch32v30x.h"
extern void by_pwm_init(void); extern void by_pwm_init(void);
extern void by_pwm_update_duty(uint32_t update_pwm_duty); void by_pwm_update_duty(uint32_t update_pwm_duty1,uint32_t update_pwm_duty2);
extern void by_pwm_power_duty(uint32_t power_pwm_duty_l, uint32_t power_pwm_duty_r); void by_pwm_power_duty(uint32_t power_pwm_duty_l1, uint32_t power_pwm_duty_r1,uint32_t power_pwm_duty_l2, uint32_t power_pwm_duty_r2);
#endif #endif

18
app/by_imu.c
View File

@@ -14,8 +14,8 @@ gyro_param_t GyroOffset;
bool GyroOffset_init = 0; bool GyroOffset_init = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17 float param_Kp = 0.17f; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004 float param_Ki = 0.004f; // 陀螺仪收敛速率的积分增益 0.004
float fast_sqrt(float x) float fast_sqrt(float x)
{ {
@@ -170,9 +170,9 @@ void ICM_getEulerianAngles(void)
float q3 = Q_info.q3; float q3 = Q_info.q3;
// 四元数计算欧拉角 // 四元数计算欧拉角
eulerAngle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch eulerAngle.pitch = asinf(-2 * q1 * q3 + 2 * q0 * q2) * 180.0f / M_PI; // pitch
eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll eulerAngle.roll = atan2f(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180.0f / M_PI; // roll
eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw eulerAngle.yaw = atan2f(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180.0f / M_PI; // yaw
/* 姿态限制*/ /* 姿态限制*/
if (eulerAngle.roll > 90 || eulerAngle.roll < -90) { if (eulerAngle.roll > 90 || eulerAngle.roll < -90) {
@@ -184,9 +184,9 @@ void ICM_getEulerianAngles(void)
} }
} }
if (eulerAngle.yaw > 360) { if (eulerAngle.yaw > 180) {
eulerAngle.yaw -= 360; eulerAngle.yaw -= 180;
} else if (eulerAngle.yaw < 0) { } else if (eulerAngle.yaw < -180) {
eulerAngle.yaw += 360; eulerAngle.yaw += 180;
} }
} }

7
app/isr.c
View File

@@ -38,6 +38,7 @@
#include "by_imu.h" #include "by_imu.h"
#include "jj_blueteeth.h" #include "jj_blueteeth.h"
#include "by_buzzer.h" #include "by_buzzer.h"
#include "jj_motion.h"
void NMI_Handler(void) __attribute__((interrupt())); void NMI_Handler(void) __attribute__((interrupt()));
void HardFault_Handler(void) __attribute__((interrupt())); void HardFault_Handler(void) __attribute__((interrupt()));
@@ -92,8 +93,8 @@ void USART1_IRQHandler(void)
void USART2_IRQHandler(void) void USART2_IRQHandler(void)
{ {
if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) { if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
uart_query_byte(UART_2,&bt_buffer); uart_query_byte(UART_2, &bt_buffer);
bt_rx_flag=true; bt_rx_flag = true;
USART_ClearITPendingBit(USART2, USART_IT_RXNE); USART_ClearITPendingBit(USART2, USART_IT_RXNE);
} }
} }
@@ -272,6 +273,8 @@ void TIM1_UP_IRQHandler(void)
{ {
if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) { if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(TIM1, TIM_IT_Update); TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
sport_motion2(bt_fly);
} }
} }

59
app/jj_blueteeth.c
View File

@@ -1,12 +1,23 @@
#include "jj_blueteeth.h" #include "jj_blueteeth.h"
#include "jj_motion.h"
#include "by_fan_control.h"
bool bt_rx_flag = false; bool bt_rx_flag = false;
uint8 bt_buffer;//接收字符存入 uint8 bt_buffer; // 接收字符存入
float bt_angle = 0.0f;
uint32_t bt_run_flag = 0;
uint8_t bt_flow_flag = 0;
uint32_t bt_fly = 0;
uint32_t bt_flow = 0;
enum bt_order { enum bt_order {
Start_work = 0x01, Start_work = 0x01,
Turn_Left = 0x02, Turn_Left = 0x02,
Turn_Right = 0x03, Turn_Right = 0x03,
Speed_up = 0x04, Speed_up = 0x04,
Speed_down = 0x05, Speed_down = 0x05,
Stop_fly = 0x06,
Flow_on = 0x08,
Flow_up = 0x09,
Flow_down = 0x10,
}; };
/** /**
* @brief 蓝牙初始化 * @brief 蓝牙初始化
@@ -15,7 +26,7 @@ enum bt_order {
void jj_bt_init() void jj_bt_init()
{ {
uart_init(UART_2, 115200, UART2_MAP1_TX_D5, UART2_MAP1_RX_D6); uart_init(UART_2, 115200, UART2_MAP1_TX_D5, UART2_MAP1_RX_D6);
// uart_tx_interrupt(UART_2, 1); // uart_tx_interrupt(UART_2, 1);
uart_rx_interrupt(UART_2, ENABLE); uart_rx_interrupt(UART_2, ENABLE);
} }
/** /**
@@ -26,24 +37,56 @@ void jj_bt_run()
if (bt_rx_flag) { if (bt_rx_flag) {
switch (bt_buffer) { switch (bt_buffer) {
case Start_work: case Start_work:
printf("1\r\n"); bt_angle = 0.0f;
break; break;
case Turn_Left: case Turn_Left:
printf("2\r\n"); bt_angle = 10.0f;
break; break;
case Turn_Right: case Turn_Right:
printf("3\r\n"); bt_angle = -10.0f;
break; break;
case Speed_down: case Speed_down:
printf("5\r\n"); bt_fly -= 10;
break; break;
case Speed_up: case Speed_up:
printf("4\r\n"); bt_fly += 10;
break;
case Stop_fly:
bt_run_flag = !bt_run_flag;
break;
case Flow_on:
bt_flow_flag = !bt_flow_flag;
break;
case Flow_up:
bt_flow += 20;
break;
case Flow_down:
bt_flow -= 20;
break; break;
default: default:
break; break;
} }
uart_write_byte(UART_2,bt_buffer);
if (bt_flow_flag == 0) {
bt_flow = 0;
}
by_pwm_update_duty(bt_flow + 500, bt_flow + 500);
bt_rx_flag = false; bt_rx_flag = false;
} }
}
void bt_printf(const char *format, ...)
{
char sbuf[40];
va_list args;
va_start(args, format);
vsnprintf(sbuf, 40, format, args);
va_end(args);
for (uint16_t i = 0; i < strlen(sbuf); i++) {
while (USART_GetFlagStatus((USART_TypeDef *)uart_index[UART_2], USART_FLAG_TC) == RESET)
;
USART_SendData((USART_TypeDef *)uart_index[UART_2], sbuf[i]);
}
} }

9
app/jj_blueteeth.h
View File

@@ -1,10 +1,13 @@
#ifndef _JJ_BLUETEETH_H_ #ifndef _JJ_BLUETEETH_H_
#define _JJ_BLUETEETH_H_ #define _JJ_BLUETEETH_H_
#include "zf_common_headfile.h" #include "stdio.h"
#include "zf_driver_uart.h" #include "zf_driver_uart.h"
extern bool bt_rx_flag; extern bool bt_rx_flag;
extern uint8 bt_buffer; extern uint8_t bt_buffer;
extern uint32_t bt_fly;
extern uint32_t bt_run_flag;
extern float bt_angle;
void jj_bt_init(); void jj_bt_init();
void jj_bt_run(); void jj_bt_run();
void bt_printf(const char *format, ...);
#endif #endif

60
app/jj_motion.c Normal file
View File

@@ -0,0 +1,60 @@
#include "jj_motion.h"
#include "by_fan_control.h"
#include "by_imu.h"
#include "../3rd-lib/PID-Library/pid.h"
#include "jj_blueteeth.h"
#include "gl_common.h"
PID_TypeDef angle_pid;
PID_TypeDef gyro_z_pid;
PID_TypeDef img_x_pid;
float an_Kp = 8.0f;
float an_Ki = 0.0f;
float an_Kd = 2.0f;
float im_Kp = 1.0f;
float im_Ki = 0.0f;
float im_Kd = 0.0f;
float set_x = 0.0f;
float img_x = 40.0f;
float yaw0 = 0;
float out_M = 0;
float out_yaw;
uint32_t cnt1 = 0;
/**
* @brief
*
* @param fy y轴的加速度
*/
void sport_motion2(uint32_t fy)
{
cnt1++;
yaw0 = imu660ra_gyro_z;
if (cnt1 >= 50) {
PID_Compute(&angle_pid);
cnt1 = 0;
}
PID_Compute(&gyro_z_pid);
if (bt_run_flag == 1) {
by_pwm_power_duty((int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M),
(int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M));
} else {
by_pwm_power_duty(500, 500, 500, 500);
}
}
/**
* @brief 结构体初始化
*
*/
void sport_pid_init()
{
PID(&angle_pid, &bt_angle, &out_yaw, &set_x, im_Kp, im_Ki, im_Kd, 0, 0);
PID(&gyro_z_pid, &yaw0, &out_M, &out_yaw, an_Kp, an_Ki, an_Kd, 1, 1);
PID_Init(&angle_pid);
PID_Init(&gyro_z_pid);
PID_SetOutputLimits(&angle_pid, -500.0f, 500.0f);
PID_SetOutputLimits(&gyro_z_pid, -400.0f, 400.0f);
PID_SetMode(&gyro_z_pid, 1);
PID_SetMode(&angle_pid, 1);
}

18
app/jj_motion.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef _JJ_MOTION_H_
#define _JJ_MOTION_H_
#include "ch32v30x.h"
#include "../3rd-lib/PID-Library/pid.h"
extern PID_TypeDef angle_pid;
extern PID_TypeDef gyro_z_pid;
extern float an_Kp;
extern float an_Ki;
extern float an_Kd;
extern float im_Kp;
extern float im_Ki;
extern float im_Kd;
extern float out_M;
extern float out_yaw;
extern float yaw0;
void sport_pid_init();
void sport_motion2(uint32_t fy);
#endif

33
app/jj_param.c
View File

@@ -1,32 +1,25 @@
#include "jj_param.h" #include "jj_param.h"
#include "./page/page_ui_widget.h" #include "./page/page_ui_widget.h"
#include "./page/page.h" #include "./page/page.h"
#include "zf_common_headfile.h"
#include "jj_motion.h"
PARAM_INFO Param_Data[DATA_NUM]; PARAM_INFO Param_Data[DATA_NUM];
soft_iic_info_struct eeprom_param; soft_iic_info_struct eeprom_param;
TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM]; TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM];
float data0 = 10.0f;
float data1 = 10.0f;
float data2 = 15;
float data3 = 100.01f;
float data4 = 1.04f;
float data5 = 4.0f;
float data6 = 5.1f;
void jj_param_eeprom_init() void jj_param_eeprom_init()
{ {
soft_iic_init(&eeprom_param, K24C02_DEV_ADDR, K24C02_SOFT_IIC_DELAY, K24C02_SCL_PIN, K24C02_SDA_PIN); // eeprom初始化 soft_iic_init(&eeprom_param, K24C02_DEV_ADDR, K24C02_SOFT_IIC_DELAY, K24C02_SCL_PIN, K24C02_SDA_PIN); // eeprom初始化
PARAM_REG(DATA0, &data0, EFLOAT, 1, "m0_p"); // 注冊 PARAM_REG(angle_Kp, &an_Kp, EFLOAT, 1, "an_P"); // 注冊
PARAM_REG(DATA1, &data1, EFLOAT, 1, "m1_p"); // 注冊 PARAM_REG(angle_Ki, &an_Ki, EFLOAT, 1, "an_I"); // 注冊
PARAM_REG(DATA2, &data2, EFLOAT, 1, "m1_i"); // 注冊 PARAM_REG(angle_Kd, &an_Kd, EFLOAT, 1, "an_D"); // 注冊
PARAM_REG(DATA3, &data3, EFLOAT, 1, "m1_d"); // 注冊 PARAM_REG(imgax_Kp, &im_Kp, EFLOAT, 1, "im_P"); // 注冊
PARAM_REG(DATA4, &data4, EFLOAT, 1, "m2_p"); // 注冊 PARAM_REG(imgax_Ki, &im_Ki, EFLOAT, 1, "im_I"); // 注冊
PARAM_REG(DATA5, &data5, EFLOAT, 1, "m2_i"); // 注冊 PARAM_REG(imgax_Kd, &im_Kd, EFLOAT, 1, "im_D"); // 注冊
PARAM_REG(DATA6, &data6, EFLOAT, 1, "m2_d"); // 注冊
for (uint8 i = 0; i < DATA_IN_FLASH_NUM ; i++) { for (uint8 i = 0; i < DATA_IN_FLASH_NUM; i++) {
soft_iic_read_8bit_registers(&eeprom_param, 4*i, (uint8 *)&iic_buffer[i], 4); soft_iic_read_8bit_registers(&eeprom_param, 4 * i, (uint8 *)&iic_buffer[i], 4);
switch (Param_Data[i].type) { switch (Param_Data[i].type) {
case EFLOAT: case EFLOAT:
*((float *)(Param_Data[i].p_data)) = *((float *)(Param_Data[i].p_data)) =
@@ -43,7 +36,7 @@ void jj_param_eeprom_init()
default: default:
break; break;
} }
system_delay_ms(10); system_delay_ms(10);
} }
} }
/** /**
@@ -66,7 +59,7 @@ void jj_param_update()
default: default:
break; break;
} }
soft_iic_write_8bit_registers(&eeprom_param, 4*i , (uint8 *)&iic_buffer[i], 4); soft_iic_write_8bit_registers(&eeprom_param, 4 * i, (uint8 *)&iic_buffer[i], 4);
system_delay_ms(10); system_delay_ms(10);
} }
} }

28
app/jj_param.h
View File

@@ -1,7 +1,9 @@
#ifndef _JJ_PARAM_H_ #ifndef _JJ_PARAM_H_
#define _JJ_PARAM_H_ #define _JJ_PARAM_H_
#include "zf_common_headfile.h" #include "stdio.h"
#include "ch32v30x.h"
#include "zf_driver_soft_iic.h"
/** /**
* @brief 注册需调参数 * @brief 注册需调参数
* *
@@ -13,15 +15,12 @@
Param_Data[_data_tag_].text = _text_; Param_Data[_data_tag_].text = _text_;
typedef enum { typedef enum {
DATA_HEAD = -1, DATA_HEAD = -1,
DATA0, angle_Kp,
DATA1, angle_Ki,
DATA2, angle_Kd,
DATA3, imgax_Kp,
DATA4, imgax_Ki,
imgax_Kd,
//
DATA5,
DATA6,
DATA_IN_FLASH_NUM, DATA_IN_FLASH_NUM,
DATA_NUM, DATA_NUM,
} data_tag_t; } data_tag_t;
@@ -36,23 +35,22 @@ typedef enum {
}ENUM_TYPE; }ENUM_TYPE;
typedef union{ typedef union{
uint32 u32; uint32_t u32;
int32 s32; int32_t s32;
float f32; float f32;
uint8 u8; uint8_t u8;
}TYPE_UNION; }TYPE_UNION;
typedef struct { typedef struct {
void *p_data; void *p_data;
ENUM_TYPE type; ENUM_TYPE type;
uint8 cmd; uint8_t cmd;
char *text; char *text;
}PARAM_INFO; }PARAM_INFO;
extern soft_iic_info_struct eeprom_param; extern soft_iic_info_struct eeprom_param;
extern PARAM_INFO Param_Data[DATA_NUM]; extern PARAM_INFO Param_Data[DATA_NUM];
extern TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM]; extern TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM];
extern float data1;
void jj_param_eeprom_init(); void jj_param_eeprom_init();
void jj_param_update(); void jj_param_update();
void jj_param_show(); void jj_param_show();

21
app/main.c
View File

@@ -29,31 +29,34 @@
#include "jj_param.h" #include "jj_param.h"
#include "./page/page_ui_widget.h" #include "./page/page_ui_widget.h"
#include "by_buzzer.h" #include "by_buzzer.h"
#include "jj_motion.h"
#include "by_imu.h"
int main(void) int main(void)
{ {
clock_init(SYSTEM_CLOCK_120M); clock_init(SYSTEM_CLOCK_120M);
system_delay_init(); system_delay_init();
debug_init(); debug_init();
// mt9v03x_init(); mt9v03x_init();
ips200_init(IPS200_TYPE_SPI); ips200_init(IPS200_TYPE_SPI);
by_gpio_init(); by_gpio_init();
by_exit_init(); by_exit_init();
by_pwm_init(); by_pwm_init();
jj_bt_init(); jj_bt_init();
by_buzzer_init(); by_buzzer_init();
// while (imu660ra_init()) while (imu660ra_init())
// ; ;
jj_param_eeprom_init(); jj_param_eeprom_init();
Page_Init(); Page_Init();
sport_pid_init();
pit_ms_init(TIM6_PIT, 2);
pit_ms_init(TIM1_PIT, 2);
// gyroOffset_init();
while (1) { while (1) {
Page_Run(); Page_Run();
jj_bt_run(); jj_bt_run();
queue_pop_read(); queue_pop_read();
bt_printf("hello:%f,%f\n",out_M,out_yaw);
if (mt9v03x_finish_flag) { if (mt9v03x_finish_flag) {
// 该操作消耗大概 1970 个 tick，折合约 110us // 该操作消耗大概 1970 个 tick，折合约 110us
memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
@@ -69,4 +72,6 @@ int main(void)
MidLineTrack(); MidLineTrack();
} }
} }
} }

17
app/page/page_param.c
View File

@@ -1,10 +1,12 @@
#include "jj_param.h" #include "jj_param.h"
#include "page_ui_widget.h" #include "page_ui_widget.h"
#include "jj_motion.h"
#include "page.h" #include "page.h"
#define LINE_HEAD 0 #define LINE_HEAD 0
#define LINE_END DATA_NUM - 2 #define LINE_END DATA_NUM - 2
static char Text[] = "RealTime Param"; static char Text[] = "RealTime Param";
int event_flag = 0; int event_flag = 0;
int32_t index_power = 0;
static int8_t Curser = LINE_HEAD; // 定义光标位置 static int8_t Curser = LINE_HEAD; // 定义光标位置
static int8_t Curser_Last = LINE_HEAD; // 定义光标位置 static int8_t Curser_Last = LINE_HEAD; // 定义光标位置
void jj_param_show(); void jj_param_show();
@@ -29,6 +31,8 @@ static void Setup()
else if (Param_Data[i].type == EFLOAT) else if (Param_Data[i].type == EFLOAT)
ips200_show_float(50, i * 18 + 2, *((float *)(Param_Data[i].p_data)), 4, 5); ips200_show_float(50, i * 18 + 2, *((float *)(Param_Data[i].p_data)), 4, 5);
} }
ips200_show_int(50, (DATA_NUM-1)* 18 + 2, index_power, 5);
} }
/** /**
@@ -70,6 +74,7 @@ static void Event(page_event event)
return; return;
} else if (page_event_press_long == event) { } else if (page_event_press_long == event) {
jj_param_update(); jj_param_update();
sport_pid_init();
Page_Shift(page_menu); Page_Shift(page_menu);
return; return;
} }
@@ -78,11 +83,11 @@ static void Event(page_event event)
} else if (Curser > LINE_END) { } else if (Curser > LINE_END) {
Curser = LINE_HEAD; Curser = LINE_HEAD;
} }
Print_Curser(Curser, Curser_Last, RGB565_PURPLE); Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
} else if (1 == event_flag) { } else if (1 == event_flag) {
if (page_event_forward == event) { if (page_event_forward == event) {
if (Param_Data[Curser].type == EFLOAT) { if (Param_Data[Curser].type == EFLOAT) {
*((float *)(Param_Data[Curser].p_data)) += 0.01f; *((float *)(Param_Data[Curser].p_data)) += powf(10.0f,(float)index_power);
} else if (Param_Data[Curser].type == EINT32) { } else if (Param_Data[Curser].type == EINT32) {
*((int32 *)(Param_Data[Curser].p_data)) += 1; *((int32 *)(Param_Data[Curser].p_data)) += 1;
} else if (Param_Data[Curser].type == EUINT32) { } else if (Param_Data[Curser].type == EUINT32) {
@@ -90,14 +95,18 @@ static void Event(page_event event)
} }
} else if (page_event_backward == event) { } else if (page_event_backward == event) {
if (Param_Data[Curser].type == EFLOAT) { if (Param_Data[Curser].type == EFLOAT) {
*((float *)(Param_Data[Curser].p_data)) -= 0.01f; *((float *)(Param_Data[Curser].p_data)) -=powf(10.0f,(float)index_power);
} else if (Param_Data[Curser].type == EINT32) { } else if (Param_Data[Curser].type == EINT32) {
*((int32 *)(Param_Data[Curser].p_data)) -= 1; *((int32 *)(Param_Data[Curser].p_data)) -= 1;
} else if (Param_Data[Curser].type == EUINT32) { } else if (Param_Data[Curser].type == EUINT32) {
*((uint32 *)(Param_Data[Curser].p_data)) -= 1; *((uint32 *)(Param_Data[Curser].p_data)) -= 1;
} }
} else if (page_event_press_short == event) { } else if (page_event_press_short == event) {
index_power++;
if (index_power > 2) {
index_power = -2;
}
ips200_show_int(50, (DATA_NUM-1)* 18 + 2, index_power, 5);
} else if (page_event_press_long == event) { } else if (page_event_press_long == event) {
event_flag = 0; event_flag = 0;
Print_Curser(Curser, Curser_Last, RGB565_PURPLE); Print_Curser(Curser, Curser_Last, RGB565_PURPLE);

4
libraries/zf_device/zf_device_imu660ra.h
View File

@@ -62,8 +62,8 @@
#if IMU660RA_USE_SOFT_IIC // 这两段 颜色正常的才是正确的 颜色灰的就是没有用的 #if IMU660RA_USE_SOFT_IIC // 这两段 颜色正常的才是正确的 颜色灰的就是没有用的
//====================================================软件 IIC 驱动==================================================== //====================================================软件 IIC 驱动====================================================
#define IMU660RA_SOFT_IIC_DELAY (10) // 软件 IIC 的时钟延时周期 数值越小 IIC 通信速率越快 #define IMU660RA_SOFT_IIC_DELAY (10) // 软件 IIC 的时钟延时周期 数值越小 IIC 通信速率越快
#define IMU660RA_SCL_PIN (B3) // 软件 IIC SCL 引脚 连接 IMU660RA 的 SCL 引脚 #define IMU660RA_SCL_PIN (E7) // 软件 IIC SCL 引脚 连接 IMU660RA 的 SCL 引脚
#define IMU660RA_SDA_PIN (B5) // 软件 IIC SDA 引脚 连接 IMU660RA 的 SDA 引脚 #define IMU660RA_SDA_PIN (E8) // 软件 IIC SDA 引脚 连接 IMU660RA 的 SDA 引脚
//====================================================软件 IIC 驱动==================================================== //====================================================软件 IIC 驱动====================================================
#else #else