first commit

This commit is contained in:
2023-12-18 14:36:22 +08:00
commit 0f3cf0952c
184 changed files with 91562 additions and 0 deletions

169
Gambal/APP/AS5600.c Normal file
View File

@ -0,0 +1,169 @@
#include "as5600.h"
#include "math.h"
#define cpr 4096
float full_rotation_offset;
long angle_data_prev;
unsigned long velocity_calc_timestamp;
float angle_prev;
/******************************************************************************/
void I2C_Init_(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//GPIOB
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
I2C_DeInit(I2C1);
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0; //I2C_OAR1
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = 400000; //400KHz
I2C_Init(I2C1, &I2C_InitStructure);
I2C_Cmd(I2C1, ENABLE);
}
/******************************************************************************/
/******************************************************************************/
#define AS5600_Address 0x36
#define RAW_Angle_Hi 0x0C
/******************************************************************************/
unsigned short I2C_getRawCount(I2C_TypeDef* I2Cx)
{
uint32_t Timeout;
unsigned short temp;
unsigned char dh,dl;
Timeout = 0xFFFF;
I2Cx->CR1 |= 0x0100;//CR1_START_Set;
/* Wait until SB flag is set: EV5 */
while ((I2Cx->SR1&0x0001) != 0x0001)
{
if (Timeout-- == 0)return 0;
}
/* Send the slave address, Reset the address bit0 for write*/
I2Cx->DR = AS5600_Address<<1;
Timeout = 0xFFFF;
/* Wait until ADDR is set: EV6 */
while ((I2Cx->SR1 &0x0002) != 0x0002)
{
if (Timeout-- == 0)return 0;
}
/* Clear ADDR flag by reading SR2 register */
temp = I2Cx->SR2;
/* Write the first data in DR register (EV8_1) */
I2Cx->DR = RAW_Angle_Hi;
/* EV8_2: Wait until BTF is set before programming the STOP */
while ((I2Cx->SR1 & 0x00004) != 0x000004);
/////////////////////////////////////////////////////////////////////////
/* Set POS bit */
I2Cx->CR1 |= 0x0800;//CR1_POS_Set;
Timeout = 0xFFFF;
/* Send START condition */
I2Cx->CR1 |= 0x0100;//CR1_START_Set;
/* Wait until SB flag is set: EV5 */
while ((I2Cx->SR1&0x0001) != 0x0001)
{
if (Timeout-- == 0)return 0;
}
Timeout = 0xFFFF;
/* Send slave address */
I2Cx->DR = (AS5600_Address<<1)+1;
/* Wait until ADDR is set: EV6 */
while ((I2Cx->SR1&0x0002) != 0x0002)
{
if (Timeout-- == 0)return 0;
}
/* EV6_1: The acknowledge disable should be done just after EV6,
that is after ADDR is cleared, so disable all active IRQs around ADDR clearing and
ACK clearing */
__disable_irq();
/* Clear ADDR by reading SR2 register */
temp = I2Cx->SR2;
/* Clear ACK */
I2Cx->CR1 &= 0xFBFF;//CR1_ACK_Reset;
/*Re-enable IRQs */
__enable_irq();
/* Wait until BTF is set */
while ((I2Cx->SR1 & 0x00004) != 0x000004);
/* Disable IRQs around STOP programming and data reading because of the limitation ?*/
__disable_irq();
/* Program the STOP */
I2C_GenerateSTOP(I2Cx, ENABLE);
/* Read first data */
dh = I2Cx->DR;
/* Re-enable IRQs */
__enable_irq();
/**/
/* Read second data */
dl = I2Cx->DR;
/* Make sure that the STOP bit is cleared by Hardware before CR1 write access */
while ((I2Cx->CR1&0x200) == 0x200);
/* Enable Acknowledgement to be ready for another reception */
I2Cx->CR1 |= 0x0400;//CR1_ACK_Set;
/* Clear POS bit */
I2Cx->CR1 &= 0xF7FF;//CR1_POS_Reset;
temp++; //useless,otherwise warning
return ((dh<<8)+dl);
}
/******************************************************************************/
void MagneticSensor_Init(void)
{
angle_data_prev = I2C_getRawCount(I2C1);
full_rotation_offset = 0;
velocity_calc_timestamp=0;
}
//<2F><>
float getAngle(void)
{
float angle_data,d_angle;
angle_data = I2C_getRawCount(I2C1);
d_angle = angle_data - angle_data_prev;
if(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
angle_data_prev = angle_data;
return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
}
/******************************************************************************/
float getVelocity(void)
{
unsigned long now_us;
float Ts, angle_c, vel;
now_us = SysTick->VAL;
if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;
else
Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6;
if(Ts == 0 || Ts > 0.5) Ts = 1e-3;
angle_c = getAngle();
vel = (angle_c - angle_prev)/Ts;
angle_prev = angle_c;
velocity_calc_timestamp = now_us;
return vel;
}
/******************************************************************************/

20
Gambal/APP/as5600.h Normal file
View File

@ -0,0 +1,20 @@
#ifndef _AS5600_H
#define _AS5600_H
#include "system.h"
#include "foc_math.h"
extern long cpr;
extern float full_rotation_offset;
extern long angle_data_prev;
extern unsigned long velocity_calc_timestamp;
extern float angle_prev;
void I2C_Init_(void);
unsigned short I2C_getRawCount(I2C_TypeDef* I2Cx);
void MagneticSensor_Init(void);
float getAngle(void);
float getVelocity(void);
#endif

75
Gambal/APP/foc_math.c Normal file
View File

@ -0,0 +1,75 @@
#include "foc_math.h"
/***************************************************************************/
// int array instead of float array
// 4x200 points per 360 deg
// 2x storage save (int 2Byte float 4 Byte )
// sin*10000
const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000};
/***************************************************************************/
// function approximating the sine calculation by using fixed size array
// ~40us (float array)
// ~50us (int array)
// precision +-0.005
// it has to receive an angle in between 0 and 2PI
float _sin(float a){
if(a < _PI_2){
//return sine_array[(int)(199.0*( a / (_PI/2.0)))];
//return sine_array[(int)(126.6873* a)]; // float array optimized
return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized
}else if(a < _PI){
// return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))];
//return sine_array[398 - (int)(126.6873*a)]; // float array optimized
return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized
}else if(a < _3PI_2){
// return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))];
//return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized
return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized
} else {
// return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))];
//return -sine_array[796 - (int)(126.6873*a)]; // float array optimized
return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized
}
}
/***************************************************************************/
// function approximating cosine calculation by using fixed size array
// ~55us (float array)
// ~56us (int array)
// precision +-0.005
// it has to receive an angle in between 0 and 2PI
float _cos(float a){
float a_sin = a + _PI_2;
a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
return _sin(a_sin);
}
/***************************************************************************/
// normalizing radian angle to [0,2PI]
float _normalizeAngle(float angle){
float a = fmod(angle, _2PI);
return a >= 0 ? a : (a + _2PI);
}
/***************************************************************************/
// Electrical angle calculation
float _electricalAngle(float shaft_angle, int pole_pairs) {
return (shaft_angle * pole_pairs);
}
/***************************************************************************/
// square root approximation function using
// https://reprap.org/forum/read.php?147,219210
// https://en.wikipedia.org/wiki/Fast_inverse_square_root
float _sqrtApprox(float number) {//low in fat
long i;
float y;
// float x;
// const float f = 1.5F; // better precision
// x = number * 0.5F;
y = number;
i = * ( long * ) &y;
i = 0x5f375a86 - ( i >> 1 );
y = * ( float * ) &i;
// y = y * ( f - ( x * y * y ) ); // better precision
return number * y;
}
/***************************************************************************/

57
Gambal/APP/foc_math.h Normal file
View File

@ -0,0 +1,57 @@
#ifndef FOCUTILS_LIB_H
#define FOCUTILS_LIB_H
#include <math.h>
/******************************************************************************/
// sign function
#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) )
#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define _sqrt(a) (_sqrtApprox(a))
#define _isset(a) ( (a) != (NOT_SET) )
// utility defines
#define _2_SQRT3 1.15470053838
#define _SQRT3 1.73205080757
#define _1_SQRT3 0.57735026919
#define _SQRT3_2 0.86602540378
#define _SQRT2 1.41421356237
#define _120_D2R 2.09439510239
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512
#define _2PI 6.28318530718
#define _3PI_2 4.71238898038
#define _PI_6 0.52359877559
/******************************************************************************/
// dq current structure
typedef struct
{
float d;
float q;
} DQCurrent_s;
// phase current structure
typedef struct
{
float a;
float b;
float c;
} PhaseCurrent_s;
// dq voltage structs
typedef struct
{
float d;
float q;
} DQVoltage_s;
/******************************************************************************/
float _sin(float a);
float _cos(float a);
float _normalizeAngle(float angle);
float _electricalAngle(float shaft_angle, int pole_pairs);
float _sqrtApprox(float number);
/******************************************************************************/
#endif

14
Gambal/APP/led.c Normal file
View File

@ -0,0 +1,14 @@
#include "led.h"
void led_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStructure);
}

12
Gambal/APP/led.h Normal file
View File

@ -0,0 +1,12 @@
#ifndef _LED_H
#define _LED_H
#define led PBout(3)
#include "stm32f10x.h"
#include "system.h"
void led_init(void);
#endif

80
Gambal/APP/pwm.c Normal file
View File

@ -0,0 +1,80 @@
#include "pwm.h"
void TIM2_PWM_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //ʹ<><CAB9>TIM2ʱ<32><CAB1>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //ʹ<><CAB9>GPIOAʱ<41><CAB1>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); //ʹ<><CAB9>GPIOAʱ<41><CAB1>
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_14;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>PA1<41><31>PA2Ϊ<32><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Ϊ50MHZ
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>TIM2<4D>ڲ<EFBFBD>ʱ<EFBFBD><CAB1>
TIM_InternalClockConfig(TIM2);
//<2F><><EFBFBD><EFBFBD>TIM2
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; //ʱ<>ӷ<EFBFBD>Ƶ
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;
TIM_TimeBaseInitStructure.TIM_Period = 1440 - 1; //ARR <20>Զ<EFBFBD><D4B6><EFBFBD>װ<EFBFBD><D7B0>ֵ
TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1; //PSC Ԥ<><D4A4>Ƶ<EFBFBD><C6B5>
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0; //
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);
//<2F><><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//<2F><><EFBFBD><EFBFBD>PWMģʽ1
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//<2F><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>Ϊ<EFBFBD>ߵ<EFBFBD>ƽ
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0; //CCR
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>1 PA0
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>2 PA1
TIM_OC3Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>3 PA2
TIM_Cmd(TIM2, ENABLE); //ʹ<>ܶ<EFBFBD>ʱ<EFBFBD><CAB1>2
}
void TIM4_1ms_Init(void) //(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_TimeBaseInitStructure.TIM_Period = 1000-1; //1ms
TIM_TimeBaseInitStructure.TIM_Prescaler= 72-1; //72<37><32>Ƶ=1MHz
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
TIM_Cmd(TIM4,ENABLE);
}
uint32_t time1_cntr;
uint32_t time2_cntr;
//<2F><>ʱ<EFBFBD><CAB1><EFBFBD>жϷ<D0B6><CFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
{
time1_cntr++;
time2_cntr++;
}
TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //<2F><><EFBFBD><EFBFBD><EFBFBD>жϱ<D0B6>־λ
}

13
Gambal/APP/pwm.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef _PWM_H
#define _PWM_H
#include "system.h"
#define M_Enable GPIO_SetBits(GPIOC,GPIO_Pin_14);
#define M_Disable GPIO_ResetBits(GPIOC,GPIO_Pin_14);
extern uint32_t time1_cntr;
extern uint32_t time2_cntr;
void TIM2_PWM_Init(void);
void TIM4_1ms_Init(void);
#endif

100
Gambal/APP/servo.c Normal file
View File

@ -0,0 +1,100 @@
#include "servo.h"
#include "Delay.h"
void Servo_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //ʹ<><CAB9>TIM2ʱ<32><CAB1>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //ʹ<><CAB9>GPIOAʱ<41><CAB1>
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_4;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>PA1<41><31>PA2Ϊ<32><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Ϊ50MHZ
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>TIM2<4D>ڲ<EFBFBD>ʱ<EFBFBD><CAB1>
TIM_InternalClockConfig(TIM2);
//<2F><><EFBFBD><EFBFBD>TIM2
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; //ʱ<>ӷ<EFBFBD>Ƶ
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD>ϼ<EFBFBD><CFBC><EFBFBD>
TIM_TimeBaseInitStructure.TIM_Period = 100 - 1; //ARR <20>Զ<EFBFBD><D4B6><EFBFBD>װ<EFBFBD><D7B0>ֵ
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1; //PSC Ԥ<><D4A4>Ƶ<EFBFBD><C6B5>
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0; //
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);
//<2F><><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//<2F><><EFBFBD><EFBFBD>PWMģʽ1
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//<2F><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>Ϊ<EFBFBD>ߵ<EFBFBD>ƽ
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0; //CCR
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>1 PA0
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>2 PA1
TIM_OC3Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>3 PA2
TIM_Cmd(TIM2, ENABLE); //ʹ<>ܶ<EFBFBD>ʱ<EFBFBD><CAB1>2
}
void Rotate(u8 a,u8 b,u8 c)
{
TIM_SetCompare1(TIM2,a);
TIM_SetCompare2(TIM2,b);
TIM_SetCompare3(TIM2,c);
}
//<2F>޸<EFBFBD>ͨ<EFBFBD><CDA8>2<EFBFBD>Ƚ<EFBFBD>ֵ
// TIM_SetCompare2(TIM2, Compare);
//<2F><>ת
void Rotate_clockwise(void)
{
Rotate(10,0,0);
delay_ms(10);
Rotate(10,10,0);
delay_ms(10);
Rotate(0,10,0);
delay_ms(10);
Rotate(0,10,10);
delay_ms(10);
Rotate(0,0,10);
delay_ms(10);
Rotate(10,0,10);
}
//<2F><>ת
void Rotate_counterclockwise(void)
{
Rotate(0,10,0);
delay_ms(10);
Rotate(10,10,0);
delay_ms(10);
Rotate(10,0,0);
delay_ms(10);
Rotate(10,0,10);
delay_ms(10);
Rotate(0,0,10);
delay_ms(10);
Rotate(0,10,10);
}
//ֹͣ
void Rotate_stop(void)
{
Rotate(0,0,0);
}
//ɲ<><C9B2>
void Rotate_shache(void)
{
Rotate(3,0,0);
}

16
Gambal/APP/servo.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef _PWM_H
#define _PWM_H
#include "system.h"
#define PWM_EN PAout(4);
void Servo_Init(void);
void PWM_SetCompare2(uint16_t Compare);
void PWM_SetCompare3(uint16_t Compare);
void Rotate(u8 a,u8 b,u8 c);
void Rotate_clockwise(void);
void Rotate_counterclockwise(void);
void Rotate_stop(void);
void Rotate_shache(void);
#endif

99
Gambal/APP/stm32_flash.c Normal file
View File

@ -0,0 +1,99 @@
#include "stm32_flash.h"
//<2F><>ȡָ<C8A1><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD>İ<EFBFBD><C4B0><EFBFBD>(16λ<36><CEBB><EFBFBD><EFBFBD>)
//faddr:<3A><><EFBFBD><EFBFBD>ַ(<28>˵<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ϊ2<CEAA>ı<EFBFBD><C4B1><EFBFBD>!!)
//<2F><><EFBFBD><EFBFBD>ֵ:<3A><>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>.
vu16 STM32_FLASH_ReadHalfWord(u32 faddr)
{
return *(vu16*)faddr;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4>
//WriteAddr:<3A><>ʼ<EFBFBD><CABC>ַ
//pBuffer:<3A><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//NumToWrite:<3A><><EFBFBD><EFBFBD>(16λ)<29><>
void STM32_FLASH_Write_NoCheck(u32 WriteAddr,u16 *pBuffer,u16 NumToWrite)
{
u16 i;
for(i=0;i<NumToWrite;i++)
{
FLASH_ProgramHalfWord(WriteAddr,pBuffer[i]);
WriteAddr+=2;//<2F><>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>2.
}
}
//<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼд<CABC><D0B4>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
//WriteAddr:<3A><>ʼ<EFBFBD><CABC>ַ(<28>˵<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ϊ2<CEAA>ı<EFBFBD><C4B1><EFBFBD>)
//pBuffer:<3A><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//NumToWrite:<3A><><EFBFBD><EFBFBD>(16λ)<29><>(<28><><EFBFBD><EFBFBD>Ҫд<D2AA><D0B4><EFBFBD><EFBFBD>16λ<36><CEBB><EFBFBD>ݵĸ<DDB5><C4B8><EFBFBD>.)
#if STM32_FLASH_SIZE<256
#define STM32_SECTOR_SIZE 1024 //<2F>ֽ<EFBFBD>
#else
#define STM32_SECTOR_SIZE 2048
#endif
u16 STM32_FLASH_BUF[STM32_SECTOR_SIZE/2];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2K<32>ֽ<EFBFBD>
void STM32_FLASH_Write(u32 WriteAddr,u16 *pBuffer,u16 NumToWrite)
{
u32 secpos; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ
u16 secoff; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>Ƶ<EFBFBD>ַ(16λ<36>ּ<EFBFBD><D6BC><EFBFBD>)
u16 secremain; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>ַ(16λ<36>ּ<EFBFBD><D6BC><EFBFBD>)
u16 i;
u32 offaddr; //ȥ<><C8A5>0X08000000<30><30><EFBFBD>ĵ<EFBFBD>ַ
if(WriteAddr<STM32_FLASH_BASE||(WriteAddr>=(STM32_FLASH_BASE+1024*STM32_FLASH_SIZE)))return;//<2F>Ƿ<EFBFBD><C7B7><EFBFBD>ַ
FLASH_Unlock(); //<2F><><EFBFBD><EFBFBD>
offaddr=WriteAddr-STM32_FLASH_BASE; //ʵ<><CAB5>ƫ<EFBFBD>Ƶ<EFBFBD>ַ.
secpos=offaddr/STM32_SECTOR_SIZE; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ
secoff=(offaddr%STM32_SECTOR_SIZE)/2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ƫ<EFBFBD><C6AB>(2<><32><EFBFBD>ֽ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ.)
secremain=STM32_SECTOR_SIZE/2-secoff; //<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD>С
if(NumToWrite<=secremain)secremain=NumToWrite;//<2F><><EFBFBD><EFBFBD><EFBFBD>ڸ<EFBFBD><DAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Χ
while(1)
{
STM32_FLASH_Read(secpos*STM32_SECTOR_SIZE+STM32_FLASH_BASE,STM32_FLASH_BUF,STM32_SECTOR_SIZE/2);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<secremain;i++)//У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if(STM32_FLASH_BUF[secoff+i]!=0XFFFF)
break;//<2F><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
}
if(i<secremain)//<2F><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
{
FLASH_ErasePage(secpos*STM32_SECTOR_SIZE+STM32_FLASH_BASE);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<secremain;i++)//<2F><><EFBFBD><EFBFBD>
{
STM32_FLASH_BUF[i+secoff]=pBuffer[i];
}
STM32_FLASH_Write_NoCheck(secpos*STM32_SECTOR_SIZE+STM32_FLASH_BASE,STM32_FLASH_BUF,STM32_SECTOR_SIZE/2);//д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
STM32_FLASH_Write_NoCheck(WriteAddr,pBuffer,secremain);//д<>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><><D6B1>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
if(NumToWrite==secremain)
break;//д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else//д<><D0B4>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
{
secpos++; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>1
secoff=0; //ƫ<><C6AB>λ<EFBFBD><CEBB>Ϊ0
pBuffer+=secremain; //ָ<><D6B8>ƫ<EFBFBD><C6AB>
WriteAddr+=secremain; //д<><D0B4>ַƫ<D6B7><C6AB>
NumToWrite-=secremain; //<2F>ֽ<EFBFBD>(16λ)<29><><EFBFBD>ݼ<EFBFBD>
if(NumToWrite>(STM32_SECTOR_SIZE/2))
secremain=STM32_SECTOR_SIZE/2;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
else
secremain=NumToWrite;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
}
}
FLASH_Lock();//<2F><><EFBFBD><EFBFBD>
}
//<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
//ReadAddr:<3A><>ʼ<EFBFBD><CABC>ַ
//pBuffer:<3A><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//NumToWrite:<3A><><EFBFBD><EFBFBD>(16λ)<29><>
void STM32_FLASH_Read(u32 ReadAddr,u16 *pBuffer,u16 NumToRead)
{
u16 i;
for(i=0;i<NumToRead;i++)
{
pBuffer[i]=STM32_FLASH_ReadHalfWord(ReadAddr);//<2F><>ȡ2<C8A1><32><EFBFBD>ֽ<EFBFBD>.
ReadAddr+=2;//ƫ<><C6AB>2<EFBFBD><32><EFBFBD>ֽ<EFBFBD>.
}
}

19
Gambal/APP/stm32_flash.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef _stm32_flash_H
#define _stm32_flash_H
#include "system.h"
//<2F>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
#define STM32_FLASH_SIZE 64 //<2F><>ѡSTM32<33><32>FLASH<53><48><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С(<28><>λΪK)
//FLASH<53><48>ʼ<EFBFBD><CABC>ַ
#define STM32_FLASH_BASE 0x08000000 //STM32 FLASH<53><48><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>ַ
vu16 STM32_FLASH_ReadHalfWord(u32 faddr);
void STM32_FLASH_Write(u32 WriteAddr,u16 *pBuffer,u16 NumToWrite); //<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼд<CABC><D0B4>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
void STM32_FLASH_Read(u32 ReadAddr,u16 *pBuffer,u16 NumToRead); //<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
#endif

131
Gambal/APP/usart1.c Normal file
View File

@ -0,0 +1,131 @@
#include "usart1.h"
//#include "math.h"
#include <stdlib.h>
#include <string.h>
u8 USART_RX_BUF[20];
//<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
unsigned short USART_RX_STA=0; //<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>־
//void USART1_IRQHandler(void) //<2F><><EFBFBD><EFBFBD>1<EFBFBD>жϳ<D0B6><CFB3><EFBFBD>
//{
// unsigned char Res;
//
// if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //
// {
// Res =USART_ReceiveData(USART1); //<2F><>ȡ<EFBFBD><C8A1><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD>ֽ<EFBFBD>
//
// if((USART_RX_STA&0x8000)==0) //<2F><><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
// {
// if(USART_RX_STA&0x4000) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD>0x0d
// {
// if(Res!=0x0a)USART_RX_STA=0; //<2F><><EFBFBD>մ<EFBFBD><D5B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¿<EFBFBD>ʼ
// else
// {
// USART_RX_STA|=0x8000; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// USART_RX_BUF[USART_RX_STA&0X3FFF]='\0'; //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ֽڷ<D6BD>'0<><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
// }
// }
// else //<2F><>û<EFBFBD>յ<EFBFBD>0x0D
// {
// if(Res==0x0d)USART_RX_STA|=0x4000;
// else
// {
// USART_RX_BUF[USART_RX_STA&0X3FFF]=Res;
// USART_RX_STA++;
// if(USART_RX_STA>(64-1))USART_RX_STA=0; //<2F><><EFBFBD>մ<EFBFBD><D5B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¿<EFBFBD>ʼ
// }
// }
// }
// }
//}
u32 RX_CNT = 0;
u8 f;
u8 x[10];
u8 y[10];
void USART1_IRQHandler(void)
{
unsigned char res;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //
{
res = USART_ReceiveData(USART1); //<2F><>ȡ<EFBFBD><C8A1><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD>ֽ<EFBFBD>
if(res != 0x0d)
{
USART_RX_BUF[RX_CNT] = res;
if(res == 'y') f = RX_CNT;
USART_RX_STA = 1;
RX_CNT++;
}
else
{
memcpy(x,USART_RX_BUF,f+1);
memcpy(y,USART_RX_BUF+f+1,20-f-1);
memset(USART_RX_BUF,0,20);
RX_CNT = 0;
}
}
}
int fputc(int ch,FILE *p) //<2F><><EFBFBD><EFBFBD>Ĭ<EFBFBD>ϵģ<CFB5><C4A3><EFBFBD>ʹ<EFBFBD><CAB9>printf<74><66><EFBFBD><EFBFBD>ʱ<EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>
{
USART_SendData(USART1,(u8)ch);
while(USART_GetFlagStatus(USART1,USART_FLAG_TXE)==RESET);
return ch;
}
//<2F><>ʼ<EFBFBD><CABC>IO <20><><EFBFBD><EFBFBD>1
//pclk1:PCLK1ʱ<31><CAB1>Ƶ<EFBFBD><C6B5>(Mhz)
//bound:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void usart1_Init(u32 bound)
{
//GPIO<49>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD>
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_8;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_9;//TX //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PA9
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_Init(GPIOA,&GPIO_InitStructure); /* <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IO */
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10;//RX //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PA10
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN_FLOATING; //ģ<><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_Init(GPIOA,&GPIO_InitStructure); /* <20><>ʼ<EFBFBD><CABC>GPIO */
//USART1 <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_InitStructure.USART_BaudRate = bound;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_InitStructure.USART_WordLength = USART_WordLength_8b;//<2F>ֳ<EFBFBD>Ϊ8λ<38><CEBB><EFBFBD>ݸ<EFBFBD>ʽ
USART_InitStructure.USART_StopBits = USART_StopBits_1;//һ<><D2BB>ֹͣλ
USART_InitStructure.USART_Parity = USART_Parity_No;//<2F><><EFBFBD><EFBFBD>żУ<C5BC><D0A3>λ
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//<2F><>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //<2F>շ<EFBFBD>ģʽ
USART_Init(USART1, &USART_InitStructure); //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
USART_Cmd(USART1, ENABLE); //ʹ<>ܴ<EFBFBD><DCB4><EFBFBD>1
USART_ClearFlag(USART1, USART_FLAG_TC);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
//Usart1 NVIC <20><><EFBFBD><EFBFBD>
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//<2F><><EFBFBD><EFBFBD>1<EFBFBD>ж<EFBFBD>ͨ<EFBFBD><CDA8>
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;//<2F><>ռ<EFBFBD><D5BC><EFBFBD>ȼ<EFBFBD>2
NVIC_InitStructure.NVIC_IRQChannelSubPriority =2; //<2F><><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>1
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQͨ<51><CDA8>ʹ<EFBFBD><CAB9>
NVIC_Init(&NVIC_InitStructure); //<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>VIC<49>Ĵ<EFBFBD><C4B4><EFBFBD>
RX_485;
}

23
Gambal/APP/usart1.h Normal file
View File

@ -0,0 +1,23 @@
#ifndef _usart1_H
#define _usart1_H
#include "system.h"
#include "stdio.h"
#define RX_485 GPIO_ResetBits(GPIOA,GPIO_Pin_8);
#define TX_485 GPIO_SetBits(GPIOA,GPIO_Pin_8);
extern float error;
extern u8 USART_RX_BUF[20];
//<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
extern unsigned short USART_RX_STA; //<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>־
extern u8 x[10];
extern u8 y[10];
void usart1_Init(u32 bound);
#endif