开源STM32自平衡小车
平衡小车开源资料网盘链接: 平衡小车百度网盘资料链接，点击进入
【哔站视频一键三连后，评论区留言邮箱获取提取码(3天内发至邮箱)】
哔站播放视频链接如下：
链接: 【开源STM32自平衡小车】哔站播放视频效果，点击进行播放
ps：如有不愿意进行自主diy的小伙伴们，我们也为其提供有推荐的套件
链接: STM32自平衡小车开源整车套件，点击即可进行查看
文章目录
提示：以下是本篇文章正文内容
推荐链接: STM32F103C8T6最小系统核心板，点击进入!
推荐链接: MPU6050姿态传感器模块，点击进入!
传感器资料链接:
https://pan.baidu.com/s/1dNDqcp76L9QdM7iSZYfz_A 提取码: 4eum
https://pan.baidu.com/s/13ZoMZ-lpAtDRhey4lKEcFQ 提取码: p7fr
推荐链接: TB6612电机驱动（双通道），点击进入!
推荐链接: 0.96寸OLED显示屏模块（128x64），点击进入!
模块资料链接：
https://pan.baidu.com/s/108smkVOLg-23cAnr37ZeGQ 密码：91t7
https://pan.baidu.com/s/1Lr8lw_6vt_VdM1UWe9CGsA 密码：p0un
推荐链接: JDY-31蓝牙无线通信模块，点击进入!
DY-31带底板资料：
https://pan.baidu.com/s/16qyiOO05whOXqYtBVBGs2Q
推荐链接: 5V和3.3V双通道输出降压模块，点击进入!（咨询客服）
推荐链接: HC-SR04超声波测距模块，点击进入!
HC-SR04资料下载：
http://pan.baidu.com/s/1sjHKDI1
HC-SR04单芯片版本资料下载接：
https://pan.baidu.com/s/1sSah9PvLBrmbA7So-6YcSw 提取码: qq35
推荐链接: 3.7V 18650锂电池，点击进入!
推荐链接: STLINK V2代码烧录器，点击进入!
STLINK资料网盘链接:
https://pan.baidu.com/s/1gxzJeDe7CJaCCl4pGnwdNQ 提取码: an2m
WIN10系统驱动解决方案
https://pan.baidu.com/s/1OQosxeUMnCe5CZkwKRKhOA 提取码: h9ii
点击文章顶部 B站视频链接 或者 点击此处 一键三连+关注，留言邮箱即可获取本小车Keil源代码文件
代码如下（main.c）：
/**********************************************************************
版权所有：源动力科技
淘 宝：https://1024tech.taobao.com/
文 件 名：mian.c
版 本：V21.01.30
摘 要：
***********************************************************************/
#include "stm32f10x.h"
#include "systick.h"
#include "led.h"
#include "iic.h"
#include "mpu6050.h"
#include "timer.h"
#include "nvic.h"
#include "imu.h"
#include "pwm.h"
#include "flash.h"
#include "pid.h"
#include "usart1.h"
#include "bluetooth.h"
#include "key.h"
#include "ps2.h"
#include "oled.h"
#include "adc.h"
#include "ultrasonic.h"
#include "imath.h"
#include "Infrared.h"
/*
* 使能SWD， 失能JTAG
* PB3,PB4,PA15作为普通IO使用
* MCU复位后，PA13/14/15 & PB3/4默认配置为JTAG功能
*/
static void _SWJ_Config(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
}
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //优先级组别2
_SWJ_Config(); //使能SWD，失能JTAG
usart1_init(115200); //串口1配置初始化
printf("usart1 is ok\r\n");
BluetoothUsart_init(9600); //蓝牙串口初始化
pwm_init(); //pwm初始
driver_pin_init(); //电机驱动配置初始化
Encoder_A_init(); //编码器A初始化
Encoder_B_init(); //编码器B初始化
tdelay_ms(100);
IIC_Init(); //IIC端口配置
mpu6050_init(); //mpu6050初始化
while(1) //mpu6050在位检测
{
uint8_t mpuId;
mpuId = get_mpu_id(); //读取mpu6050的id
if(mpuId==0x68||mpuId==0x98) //判断mpu6050的ID是否正确
break;
tdelay_ms(50);
}
get_iir_factor(&Mpu.att_acc_factor,0.005f,25); //姿态解算时加速度低通系数
OLED_Init(); //OLED显示屏端口初始化
adc_init(); //ADC配置初始化
ReadCalData(); //读取校准后的陀螺仪零偏数据
tdelay_ms(100);
timer_init(); //定0时器初始化
HCSR04_Init(); //超声波端口初始化
systick_init(); //滴答定时器初始化
NVIC_config(); //中断配置初始化
while(1)
{
if(softTimes[0] == 0) //100ms
{
softTimes[0] = 20;
}
if(softTimes[1] == 0) //20ms
{
softTimes[1] = 4;
UltrasonicWave_StartMeasure(); //超声波发出起始高电平
ParseBluetoothMessage(USARTxBlueTooth_RX_DATA, &BluetoothParseMsg); //蓝牙数据包解析
}
if(softTimes[2] == 0) //100ms
{
softTimes[2] = 20;
voltage_detection(); //低电压检测
OledShowDatas();
}
}
}
代码如下（SysTick_Handler() ）：
/**
* @brief This function handles SysTick Handler.
* @param None
* @retval None
*/
void SysTick_Handler(void)
{
softTimesCountdown(); //软件定时倒计时
UltrasonicCheck(); //超声波在位检测
get_gyro_raw(); //陀螺仪数据
get_deg_s(&gyro_raw_f,&Mpu.deg_s); //陀螺仪原始数据转为度为单位的速率
get_rad_s(&gyro_raw_f,&Mpu.rad_s); //陀螺仪原始数据转为弧度为单位的速率
get_acc_raw(); //加速度数据
acc_iir_lpf(&acc_raw_f,&acc_att_lpf,Mpu.att_acc_factor); //姿态解算时加速度低通滤波
get_acc_g(&acc_att_lpf,&Mpu.acc_g);
//姿态解算
mahony_update(Mpu.rad_s.x,Mpu.rad_s.y,Mpu.rad_s.z,Mpu.acc_g.x,Mpu.acc_g.y,Mpu.acc_g.z);
Matrix_ready(); //姿态解算相关矩阵更新
encoder_val_a = read_encoder(ENCODER_A); //A编码器值读取
encoder_manage(&encoder_val_a); //编码器值处理
encoder_val_b = read_encoder(ENCODER_B); //B编码器值读取
encoder_manage(&encoder_val_b); //编码器值处理
ctr.pwm[0] = ctr_bal(att.rol,Mpu.deg_s.y); //角度直立平衡控制器
ctr.pwm[1] = ctr_vel(encoder_val_a,-encoder_val_b); //速度控制器
ctr.pwm[2] = ctr_turn(encoder_val_a,-encoder_val_b,Mpu.deg_s.z); //转向控制器
ctr.out[0] = ctr.pwm[0] + ctr.pwm[1] + ctr.pwm[2]; //电机1匹配输出
ctr.out[1] = ctr.pwm[0] + ctr.pwm[1] - ctr.pwm[2]; //电机2匹配输出
i_limit(&(ctr.out[0]),AMPLITUDE); //输出限幅
i_limit(&(ctr.out[1]),AMPLITUDE); //输出限幅
dir_config(&(ctr.out[0]),&(ctr.out[1])); //根据正负设置方向
_ctr_out(); //控制器输出
ANO_DMA_SEND_DATA(); //地面站波形显示
gyro_cal(&gyro_raw_cal); //陀螺仪零偏校准
}
代码如下（ systick_init ）：
/*
* 函数名：systick_init
* 描述 ：系统滴答定时器配置初始化
* 输入 ：
* 返回 ：
*/
void systick_init(void)
{
SystemCoreClockUpdate();
//时钟频率：72Mhz ， 每秒可以计数72000000次，一次的时间则为：1/72000（ms），10ms需要的次数：10/(1/72000) = 720000 -> 5ms需要的次数则为：360000
if (SysTick_Config(SystemCoreClock / 200)) //1000 -> 1ms
{
/* Capture error */
while (1);
}
SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; //使能
}
代码如下（ ANO_DMA_SEND_DATA ）：
vvoid ANO_DMA_SEND_DATA(void)
{
static uint8_t ANO_debug_cnt = 0;
ANO_debug_cnt++;
if(ANO_debug_cnt==1)
{
ANO_DT_Send_Status();
}
else if(ANO_debug_cnt==2)
{
ANO_DT_Send_Senser((int16_t)acc_raw.x,(int16_t)acc_raw.y,(int16_t)acc_raw.z,
(int16_t)gyro_raw.x,(int16_t)gyro_raw.y,(int16_t)gyro_raw.z,
(int16_t)0,(int16_t)0,(int16_t)0);
}
else if(ANO_debug_cnt==3)
{
ANO_DT_Send_RCData(1100,1200,1300,1400,1500,1600,1700,1800,1900,1100);
}
else if(ANO_debug_cnt==4)
{
ANO_DT_Send_Power(13.52,57.63);
}
else if(ANO_debug_cnt==5)
{
ANO_DT_Send_User(0,0,0,0,0,
encoder_val_a, encoder_val_b,ctr.pwm[0],ctr.pwm[1],acc_raw_f.z,
0,0,0,0,0);
}
else if(ANO_debug_cnt==6)
{
if(f.send_pid1)
{
f.send_pid1 = 0;
//向上位机发送内环速度环PID参数值
ANO_DT_Send_PID(1, vel.kp*0.1f,
vel.ki,
vel.kd,
1.5f,
2.5f,
1.5f,
2.5f,
1.5f,
2.5f);
}
if(f.send_pid2)
{
f.send_pid2 = 0;
//向上位机发送外环角度环PID参数值
ANO_DT_Send_PID(2, bal.kp*0.1f,
bal.ki,
bal.kd,
2.6f,
1.5f,
2.4f,
1.1f,
2.2f,
1.1f);
}
if(CalGyro.success==1) //返回校准成功信息给上位机
{
CalGyro.success = 0;
ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
}
else if(CalGyro.success==2) //反馈校准失败信息给上位机
{
CalGyro.success = 0;
ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
}
}
else if(ANO_debug_cnt==7)
{
ANO_debug_cnt = 0;
}
}
代码如下：
/**********************************************************************
版权所有：源动力科技
淘 宝：https://1024tech.taobao.com/
文 件 名：adc.c
版 本：V21.01.30
摘 要：
***********************************************************************/
#include "adc.h"
_ADC_VALUE bat = {0 ,0 };
uint16_t adc_value[1];
/* 端口配置初始化 */
void adc_gpio_init(void)
{
GPIO_InitTypeDef GPIO_initStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_initStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_initStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA,&GPIO_initStructure);
}
/* adc配置 */
void adc_config(void)
{
ADC_InitTypeDef ADC_initStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,ENABLE);
ADC_initStructure.ADC_ContinuousConvMode = ENABLE; //连续转换
ADC_initStructure.ADC_DataAlign = ADC_DataAlign_Right; //数据右对齐
ADC_initStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //软件触发
ADC_initStructure.ADC_Mode = ADC_Mode_Independent; //独立工作模式
ADC_initStructure.ADC_NbrOfChannel = 1; //顺序进行规则转换的ADC通道的数目 1-16
ADC_initStructure.ADC_ScanConvMode = DISABLE; //扫描模式：单次模式
ADC_Init(ADC1,&ADC_initStructure);
ADC_Cmd(ADC1,ENABLE);
ADC_DMACmd(ADC1,ENABLE);
RCC_ADCCLKConfig(RCC_PCLK2_Div8); //ADC时钟分频
//通道配置，采样时间设置
ADC_RegularChannelConfig(ADC1,ADC_Channel_2,1,ADC_SampleTime_239Cycles5);
ADC_ResetCalibration(ADC1); //复位校准
while(ADC_GetCalibrationStatus(ADC1)); //等待
ADC_StartCalibration(ADC1); //启动校准
while(ADC_GetCalibrationStatus(ADC1)); //等待校准完成
ADC_SoftwareStartConvCmd(ADC1,ENABLE); //开启转换
}
void dma_config(void)
{
DMA_InitTypeDef DMA_initStructure;
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,ENABLE);
DMA_initStructure.DMA_BufferSize = 1; //缓存的数据大小
DMA_initStructure.DMA_DIR = DMA_DIR_PeripheralSRC; //传输方向：外设->内存
DMA_initStructure.DMA_M2M = DMA_M2M_Disable;
DMA_initStructure.DMA_MemoryBaseAddr = (u32)adc_value; //内存基地址
DMA_initStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; //DMA传输的内存数据大小：半字为单位
DMA_initStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址自增
DMA_initStructure.DMA_Mode = DMA_Mode_Circular; //循环模式
DMA_initStructure.DMA_PeripheralBaseAddr = ((u32)&ADC1->DR); //外设地址
DMA_initStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //DMA传输的外设数据大小：半字为单位
DMA_initStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设地址不变
DMA_initStructure.DMA_Priority = DMA_Priority_Medium; //中等优先级
DMA_Init(DMA1_Channel1,&DMA_initStructure);
DMA_Cmd(DMA1_Channel1,ENABLE);
}
void adc_init(void)
{
adc_gpio_init();
adc_config();
dma_config();
}
/*
* 函数名：voltage_detection
* 描述 ：电源电压采集
* 输入 ：
* 返回 ：
*/
void voltage_detection(void)
{
bat.adc = adc_value[0];
bat.voltage = (float)(bat.adc)/4096.0f*3.3f*11.0f; //实际电路采用电阻10:1比例进行分压
if(bat.voltage<=7.0f)
{
bat.danger_flag = 1;
}
else
{
bat.danger_flag = 0;
}
}
代码如下：
/*
* 函数名：gyro_cal
* 描述 ：陀螺仪零偏数据校准
* 输入 ：gyro_in原始的静止时陀螺仪数据首地址
* 返回 ：
*/
void gyro_cal(SI_F_XYZ *gyro_in)
{
if(CalGyro.flag==1 && 1)
{
if(CalGyro.i < GyroCalSumValue) //原始静止数据500次累加求取平均
{
CalGyro.offset.x += gyro_in->x;
CalGyro.offset.y += gyro_in->y;
CalGyro.offset.z += gyro_in->z;
CalGyro.i++;
}
else
{
CalGyro.i = 0;
CalGyro.OffsetFlashWrite.x = CalGyro.offset.x / GyroCalSumValue; //得到三轴的零偏
CalGyro.OffsetFlashWrite.y = CalGyro.offset.y / GyroCalSumValue; //得到三轴的零偏
CalGyro.OffsetFlashWrite.z = CalGyro.offset.z / GyroCalSumValue; //得到三轴的零偏
/* 将陀螺仪零偏写入flash */
FlashWriteNineFloat(SENSOR_CAL_ADDRESS, 0,0,0,
0,0,0,
CalGyro.OffsetFlashWrite.x,CalGyro.OffsetFlashWrite.y,CalGyro.OffsetFlashWrite.z);
/* 校准完数据之后立马读取出来进行使用 */
flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS, &CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.OffsetFlashRead.x,
&CalGyro.OffsetFlashRead.y,
&CalGyro.OffsetFlashRead.z);
/* 判断是否正确读出 */
if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
{
_set_val(&gyro_offset,&CalGyro.OffsetFlashRead); //陀螺仪零偏设置
CalGyro.success = 1; //校准成功
_led.sta = 0;
}
else
{
CalGyro.success = 2; //校准失败
}
CalGyro.offset.x = 0;
CalGyro.offset.y = 0;
CalGyro.offset.z = 0;
CalGyro.flag = 0; //校准标志位清除
}
}
}
/*
* 函数名：ReadCalData
* 描述 ：陀螺仪零偏校准数据进行读取
* 输入 ：
* 返回 ：err
*/
int ReadCalData(void)
{
ErrorStatus err;
flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS, &CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.None.x,
&CalGyro.None.y,
&CalGyro.None.z,
&CalGyro.OffsetFlashRead.x,
&CalGyro.OffsetFlashRead.y,
&CalGyro.OffsetFlashRead.z);
/* 判断是否正确读出 */
if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
{
_set_val(&gyro_offset, &CalGyro.OffsetFlashRead); //陀螺仪零偏设置
err = SUCCESS ;
}
else
{
err = ERROR;
}
return err;
}
代码如下：
float getDistance;
uint16_t EchoFlag = 0;
uint16_t disCounter;
void EXTI15_10_IRQHandler(void)
{
disCounter = 0;
tdelay_us(3); //延时10us
if(EXTI_GetITStatus(EXTI_Line13) != RESET)
{
EchoFlag = 1;
TIM_SetCounter(TIM1,0);
TIM_Cmd(TIM1, ENABLE);
while(GPIO_ReadInputDataBit(HCSR04_ECHO_PORT,HCSR04_ECHO_PIN)) //等待低电平
{
disCounter = TIM_GetCounter(TIM1);
if(disCounter>5000) //最远测量控制在5000计数值，换为距离为85cm
break;
}
TIM_Cmd(TIM1, DISABLE);
getDistance = TIM_GetCounter(TIM1)/1000000.0f * 340.0f / 2.0f * 100.0f; //us->s 声速340m/s 最终转为厘米cm
//计算公式为：距离=（高电平时间*340m/s）/2
// printf("%.3fcm %d\r\n",getDistance,TIM_GetCounter(TIM1));
EXTI_ClearITPendingBit(EXTI_Line13); //清除EXTI7线路挂起位
}
}
代码如下（ mahony_update(float gx, float gy, float gz, float ax, float ay, float az) ）：
/**********************************************************************
版权所有：源动力科技
淘 宝：https://1024tech.taobao.com/
文 件 名：imu.c
版 本：V21.01.30
摘 要：
***********************************************************************/
#include "imu.h"
#include "imath.h"
#include "math.h"
#include "mpu6050.h"
#include "timer.h"
_Matrix Mat = {0};
_Attitude att = {0};
#define mahonyPERIOD 5.0f //姿态解算周期（ms）
#define kp 0.5f //proportional gain governs rate of convergence to accelerometer/magnetometer
#define ki 0.0001f //integral gain governs rate of convergenceof gyroscope biases
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //quaternion elements representing theestimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; //scaled integral error
/*
* 函数名：mahony_update
* 描述 ：姿态解算
* 输入 ：陀螺仪三轴数据（单位：弧度/秒），加速度三轴数据（单位：g）
* 返回 ：
*/
//Gyroscope units are radians/second, accelerometer units are irrelevant as the vector is normalised.
void mahony_update(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
if(ax*ay*az==0)
return;
//[ax,ay,az]是机体坐标系下加速度计测得的重力向量(竖直向下)
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//VectorA = MatrixC * VectorB
//VectorA ：参考重力向量转到在机体下的值
//MatrixC ：地理坐标系转机体坐标系的旋转矩阵
//VectorB ：参考重力向量（0,0,1）
//[vx,vy,vz]是地理坐标系重力分向量[0,0,1]经过DCM旋转矩阵(C(n->b))计算得到的机体坐标系中的重力向量(竖直向下)
vx = Mat.DCM_T[0][2];
vy = Mat.DCM_T[1][2];
vz = Mat.DCM_T[2][2];
//机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’
//利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
//实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
//但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
//两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
//加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
//也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。
ex = ay*vz - az*vy;
ey = az*vx - ax*vz;
ez = ax*vy - ay*vx;
//对误差向量进行积分
exInt = exInt + ex*ki;
eyInt = eyInt + ey*ki;
ezInt = ezInt + ez*ki;
//姿态误差补偿到角速度上，修正角速度积分漂移，通过调节Kp、Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
gx = gx + kp*ex + exInt;
gy = gy + kp*ey + eyInt;
gz = gz + kp*ez + ezInt;
//一阶龙格库塔法更新四元数
q0 = q0 + (-q1*gx - q2*gy - q3*gz)* mahonyPERIOD * 0.0005f;
q1 = q1 + ( q0*gx + q2*gz - q3*gy)* mahonyPERIOD * 0.0005f;
q2 = q2 + ( q0*gy - q1*gz + q3*gx)* mahonyPERIOD * 0.0005f;
q3 = q3 + ( q0*gz + q1*gy - q2*gx)* mahonyPERIOD * 0.0005f;
//把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
//四元素转欧拉角
att.pit = atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * rad_to_angle;
att.rol = asin(2.0f*(q0*q2 - q1*q3)) * rad_to_angle;
//z轴角速度积分的偏航角
att.yaw += Mpu.deg_s.z * mahonyPERIOD * 0.001f;
}
/*
* 函数名：rotation_matrix
* 描述 ：旋转矩阵：机体坐标系 -> 地理坐标系
* 输入 ：
* 返回 ：
*/
void rotation_matrix(void)
{
Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);
Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);
Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}
/*
* 函数名：rotation_matrix_T
* 描述 ：旋转矩阵的转置矩阵：地理坐标系 -> 机体坐标系
* 输入 ：
* 返回 ：
*/
void rotation_matrix_T(void)
{
Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);
Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2);
Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);
Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}
/*
* 函数名：Matrix_ready
* 描述 ：矩阵更新准备，为姿态解算使用
* 输入 ：
* 返回 ：
*/
void Matrix_ready(void)
{
rotation_matrix(); //旋转矩阵更新
rotation_matrix_T(); //旋转矩阵的逆矩阵更新
}
代码如下：
/*
ctr.bais[0]：角度控制器偏差存储
ctr.bais[1]：速度控制器偏差累计存储
ctr.bais[2]：转向控制器偏差存储
ctr.exp[0]：角度控制器期望角度
ctr.exp[1]：速度控制器期望速度
ctr.exp[2]：转向控制器期望角度
*/
/*
* 函数名：ctr_bal
* 描述 ：角度PD控制器
* 输入 ：angle当前角度，gyro当前角速度
* 返回 ：PID控制器输出
*/
int ctr_bal(float angle,float gyro)
{
ctr.exp[0] = 0; //期望角度
ctr.bais[0] = (float)(angle - ctr.exp[0]); //角度偏差
ctr.balance = bal.kp * ctr.bais[0] + gyro * bal.kd; //角度平衡控制
return ctr.balance;
}
int CarStepForwardOrBackward;
/*
* 函数名：ctr_vel
* 描述 ：速度PI控制器
* 输入 ：encoder_left编码器值A，encoder_right编码器值B
* 返回 ：PID控制器输出
*/
int ctr_vel(int encoder_left,int encoder_right)
{
static float encoder_cur,encoder_last;
CarStepForwardOrBackward = CarSpeedCtrlForwardOrBackward(1); //遥控器控制的前后方向速度
remote.ForwardOrBackward = remoteCarForwardOrBackward( (float)CarStepForwardOrBackward , MODE_BLUETEETH , UltraSuccess(BluetoothParseMsg.ModeRocker) ); //遥控器控制前后方向运动
encoder_last = ((encoder_left) + (encoder_right)) - 0; //速度误差
encoder_cur = encoder_cur * 0.8 + encoder_last * 0.2; //对偏差进行低通滤波
ctr.bais[1] += encoder_cur; //偏差累加和
ctr.bais[1] = ctr.bais[1] - remote.ForwardOrBackward; //遥控控制前后方向
if(ctr.bais[1] > 10000) ctr.bais[1] = 10000; //限幅
if(ctr.bais[1] <-10000) ctr.bais[1] =-10000;
ctr.velocity = encoder_cur * vel.kp + ctr.bais[1] * vel.ki; //速度控制
return ctr.velocity;
}
代码如下（ ParseBluetoothMessage(char *pInput ,uint16_t rcLens , BluetoothParse *blueParseMsg ) ）：
*
* 函数名：USART3_IRQHandler
* 描述 ：蓝牙串口接收中断处理函数
* 输入 ：
* 返回 ：
*/
void USART3_IRQHandler(void)
{
if(USART_GetITStatus(BLUETOOTH_USARTx, USART_IT_RXNE) != RESET)
{
if(BlueToothBufIndex > USARTxBlueTooth_RX_LEN)
BlueToothBufIndex = 0;
USARTxBlueTooth_RX_DATA[BlueToothBufIndex++] = USART_ReceiveData(BLUETOOTH_USARTx);
BlueToothNumbers ++;
USART_ClearITPendingBit(BLUETOOTH_USARTx, USART_IT_RXNE); //清除空闲中断标志
}
}
/*
* 函数名：ParseBluetoothMessage
* 描述 ：解析蓝牙app发送过来的数据包
* 输入 ：pInput收到的蓝牙遥控数据首地址 , blueParseMsg解析后保存的蓝牙数据有效信息
* 返回 ：err
*/
/*
* 协议形式：字符串
* 帧头：# 帧尾：*
* 例如：#9,1,1,2,2,1*
* 每位代表含义：9：数据长度(固定) 1：模式 1：关闭/开始状态 2：蓝牙摇杆X轴数据 2：蓝牙摇杆Y轴数据 1：校验值(累加和减去7)
*/
uint8_t ParseBluetoothMessage(char *pInput , BluetoothParse *blueParseMsg)
{
unsigned char plen,sum,check;
ErrorStatus err;
char *pdata = pInput;
while(( pdata-pInput ) < BlueToothBufIndex )
{
if(*pdata == '#')// #9,1,1,2,2,1* //接收到数据的包头#
{
plen = (unsigned char)atof(strtok(pdata+1, ",")); //解析长度length
if(plen == 9)
{
/* 将读出的数据进行累加校验 */
sum = (unsigned char)int_abs( ((int)atof(strtok(pdata+3, ",")) +
(int)atof(strtok(NULL, ",")) +
(int)atof(strtok(NULL, ",")) +
(int)atof(strtok(NULL, ","))) - 7 );
/* 读出累加校验数据 */
check = (unsigned char)atof(strtok(NULL, ",")) ; //累加校验数据
if(sum == check) //校验匹配成功才进行数据解析
{
blueParseMsg->ModeRocker = (unsigned char)atof(strtok(pdata+3, ",")); //模式数据
blueParseMsg->OpenCloseStatus = (unsigned char)atof(strtok(pdata+5, ",")); //关闭/开始状态数据
blueParseMsg->Xrocker = (unsigned char)atof(strtok(pdata+7, ",")); //摇杆X数据
blueParseMsg->Yrocker = (unsigned char)atof(strtok(pdata+9, ",")); //摇杆Y数据
err = SUCCESS;
}
else
{
err = ERROR;
}
}
else
{
err = ERROR;
}
}
else
{
err = ERROR;
}
pdata++;
}
BlueToothBufIndex = 0;
return err ;
}
【 PS：点击文章顶部 B站视频链接 或者 点击此处 一键三连+关注，留言邮箱即可获取本小车原理图纸文件。】
我正在学习如何使用Nokogiri，根据这段代码我遇到了一些问题:require'rubygems'require'mechanize'post_agent=WWW::Mechanize.newpost_page=post_agent.get('http://www.vbulletin.org/forum/showthread.php?t=230708')puts"\nabsolutepathwithtbodygivesnil"putspost_page.parser.xpath('/html/body/div/div/div/div/div/table/tbody/tr/td/div
总的来说，我对ruby还比较陌生，我正在为我正在创建的对象编写一些rspec测试用例。许多测试用例都非常基础，我只是想确保正确填充和返回值。我想知道是否有办法使用循环结构来执行此操作。不必为我要测试的每个方法都设置一个assertEquals。例如:describeitem,"TestingtheItem"doit"willhaveanullvaluetostart"doitem=Item.new#HereIcoulddotheitem.name.shouldbe_nil#thenIcoulddoitem.category.shouldbe_nilendend但我想要一些方法来使用
关闭。这个问题是opinion-based.它目前不接受答案。想要改进这个问题？更新问题，以便editingthispost可以用事实和引用来回答它.关闭4年前。Improvethisquestion我想在固定时间创建一系列低音和高音调的哔哔声。例如:在150毫秒时发出高音调的蜂鸣声在151毫秒时发出低音调的蜂鸣声200毫秒时发出低音调的蜂鸣声250毫秒的高音调蜂鸣声有没有办法在Ruby或Python中做到这一点？我真的不在乎输出编码是什么(.wav、.mp3、.ogg等等)，但我确实想创建一个输出文件。
给定这段代码defcreate@upgrades=User.update_all(["role=?","upgraded"],:id=>params[:upgrade])redirect_toadmin_upgrades_path,:notice=>"Successfullyupgradeduser."end我如何在该操作中实际验证它们是否已保存或未重定向到适当的页面和消息？ 最佳答案 在Rails3中，update_all不返回任何有意义的信息，除了已更新的记录数(这可能取决于您的DBMS是否返回该信息)。http://ar.ru
我在我的项目目录中完成了compasscreate.和compassinitrails。几个问题:我已将我的.sass文件放在public/stylesheets中。这是放置它们的正确位置吗？当我运行compasswatch时，它不会自动编译这些.sass文件。我必须手动指定文件:compasswatchpublic/stylesheets/myfile.sass等。如何让它自动运行？文件ie.css、print.css和screen.css已放在stylesheets/compiled。如何在编译后不让它们重新出现的情况下删除它们？我自己编译的.sass文件编译成compiled/t
我正在寻找执行以下操作的正确语法(在Perl、Shell或Ruby中):#variabletoaccessthedatalinesappendedasafileEND_OF_SCRIPT_MARKERrawdatastartshereanditcontinues. 最佳答案 Perl用__DATA__做这个:#!/usr/bin/perlusestrict;usewarnings;while(){print;}__DATA__Texttoprintgoeshere 关于ruby-如何将脚
Rackup通过Rack的默认处理程序成功运行任何Rack应用程序。例如:classRackAppdefcall(environment)['200',{'Content-Type'=>'text/html'},["Helloworld"]]endendrunRackApp.new但是当最后一行更改为使用Rack的内置CGI处理程序时，rackup给出“NoMethodErrorat/undefinedmethod`call'fornil:NilClass”:Rack::Handler::CGI.runRackApp.newRack的其他内置处理程序也提出了同样的反对意见。例如Rack
在选择我想要运行操作的频率时，唯一的选项是“每天”、“每小时”和“每10分钟”。谢谢!我想为我的Rails3.1应用程序运行调度程序。 最佳答案 这不是一个优雅的解决方案，但您可以安排它每天运行，并在实际开始工作之前检查日期是否为当月的第一天。 关于ruby-如何每月在Heroku运行一次Scheduler插件？，我们在StackOverflow上找到一个类似的问题： https://stackoverflow.com/questions/8692687/
我有一个对象has_many应呈现为xml的子对象。这不是问题。我的问题是我创建了一个Hash包含此数据，就像解析器需要它一样。但是rails自动将整个文件包含在.........我需要摆脱type="array"和我该如何处理？我没有在文档中找到任何内容。 最佳答案 我遇到了同样的问题；这是我的XML:我在用这个:entries.to_xml将散列数据转换为XML，但这会将条目的数据包装到中所以我修改了:entries.to_xml(root:"Contacts")但这仍然将转换后的XML包装在“联系人”中，将我的XML代码修改为
我有一大串格式化数据(例如JSON)，我想使用Psychinruby同时保留格式转储到YAML。基本上，我希望JSON使用literalstyle出现在YAML中:---json:|{"page":1,"results":["item","another"],"total_pages":0}但是，当我使用YAML.dump时，它不使用文字样式。我得到这样的东西:---json:!"{\n\"page\":1,\n\"results\":[\n\"item\",\"another\"\n],\n\"total_pages\":0\n}\n"我如何告诉Psych以想要的样式转储标量？解