12.22

Luo25177 · Dec 22, 2023 · 4050b5e · 4050b5e
1 parent 8e81d7f
commit 4050b5e
Show file tree

Hide file tree

Showing 10 changed files with 46 additions and 64 deletions.
diff --git a/Chassis/src/leg.c b/Chassis/src/leg.c
@@ -24,9 +24,7 @@ void legInit(Leg* leg, int dir, DJmotor* wheel, Tmotor* front, Tmotor* behind) {
 	leg->TFset								= 0;
 	leg->TBset								= 0;
 	leg->TWheelset						= 0;
-
 	leg->timer								= 0;
-
 	leg->normalforce					= 0;
 	// 电机绑定
 	leg->front->initSetAngle	= FrontAngleInit;
@@ -47,10 +45,9 @@ void legUpdate(Leg* leg) {
 	leg->angle4		 = (float) (leg->behind->initSetAngle + (leg->behind->real.angleRad * leg->dir));
 	leg->dis.now	 = (float) leg->wheel->real.angleRad * WHEELR * leg->dir;
 	leg->dis.dot	 = (float) leg->wheel->real.velocity * RPMTORAD * WHEELR * leg->dir;
-
-	leg->TFnow		 = leg->front->real.torque * leg->dir;
-	leg->TBnow		 = leg->behind->real.torque * leg->dir;
-	leg->TWheelnow = leg->wheel->real.torque * leg->dir;
+	leg->TFnow		 = (float) leg->front->real.torque * leg->dir;
+	leg->TBnow		 = (float) leg->behind->real.torque * leg->dir;
+	leg->TWheelnow = (float) leg->wheel->real.torque * leg->dir;
 	INVMC(leg);
 }
 
@@ -140,7 +137,6 @@ void VMC(Leg* leg) {
 	leg->TBset				= trans[1][0] * leg->Fset + trans[1][1] * leg->Tpset;
 }
 
-// TODO: 没测试过不一定没问题
 //----
 // @brief 逆向VMC解算 实际电机扭矩->虚拟力
 //

diff --git a/Hardware/src/tim.c b/Hardware/src/tim.c
@@ -2,6 +2,8 @@
 
 unsigned int GlobalTimer = 0;
 
+// 时钟计算公式 时钟频率 / TIM_Prescaler / TIM_Period
+
 void tim2Init() {
 	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 	NVIC_InitTypeDef				NVIC_InitStructure;
@@ -14,7 +16,7 @@ void tim2Init() {
 	NVIC_InitStructure.NVIC_IRQChannelCmd								 = ENABLE;
 	NVIC_Init(&NVIC_InitStructure);
 
-	TIM_TimeBaseStructure.TIM_Prescaler			= 84 - 1;		 // 84MHZ
+	TIM_TimeBaseStructure.TIM_Prescaler			= 84 - 1;
 	TIM_TimeBaseStructure.TIM_Period				= 1000 - 1;	 // 1ms
 	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
 	TIM_TimeBaseStructure.TIM_CounterMode		= TIM_CounterMode_Up;
@@ -38,7 +40,7 @@ void tim3Init() {
 	NVIC_InitStructure.NVIC_IRQChannelCmd								 = ENABLE;
 	NVIC_Init(&NVIC_InitStructure);
 
-	TIM_TimeBaseStructure.TIM_Prescaler			= 84 - 1;			 // 84MHZ，分频系数
+	TIM_TimeBaseStructure.TIM_Prescaler			= 84 - 1;
 	TIM_TimeBaseStructure.TIM_Period				= 100000 - 1;	 // 100ms
 	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
 	TIM_TimeBaseStructure.TIM_CounterMode		= TIM_CounterMode_Up;

diff --git a/Hardware/src/usart.c b/Hardware/src/usart.c
@@ -97,7 +97,7 @@ void usart2Init() {
 	GPIO_InitStructure.GPIO_PuPd	= GPIO_PuPd_NOPULL;
 	GPIO_Init(GPIOA, &GPIO_InitStructure);
 
-	USART_InitStructure.USART_BaudRate						= 115200;
+	USART_InitStructure.USART_BaudRate						= 921600;
 	USART_InitStructure.USART_WordLength					= USART_WordLength_8b;
 	USART_InitStructure.USART_stopBits						= USART_stopBits_1;
 	USART_InitStructure.USART_Parity							= USART_Parity_No;

diff --git a/Main/src/main.c b/Main/src/main.c
@@ -67,7 +67,7 @@ static void taskBeep(void* pdata) {
 }
 
 //----
-// @brief 开始运行
+// @brief 开始运行 	0.67215ms
 //
 // @param pdata
 //----
@@ -84,7 +84,7 @@ static void taskRun(void* pdata) {
 			djmotor[1].monitor.enable = true;
 			robotRun();
 		}
-		OSTimeDly(100);
+		OSTimeDly(200);
 	}
 }
 

diff --git a/Motor/src/vesc.c b/Motor/src/vesc.c
@@ -44,7 +44,7 @@ void VESCInit(VESC* vesc, u8 id) {
 		vesc[i].set.torque					= 0;
 		vesc[i].param.duty					= 0.1;
 		vesc[i].id									= id++;
-		// TODO:
+		// TODO: 电机参数
 		vesc[i].param.polepairs			= VESCU10POLEPAIR;
 	}
 }

diff --git a/PRO/template.uvguix.belov b/PRO/template.uvguix.belov
diff --git a/PRO/template.uvoptx b/PRO/template.uvoptx
@@ -130,7 +130,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>CMSIS_AGDI</Key>
-          <Name>-X"Any" -UAny -O718 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8000 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F405RG$Flash\STM32F4xx_1024.FLM)</Name>
+          <Name>-X"Any" -UAny -O718 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65555 -TC168000000 -TT10000000 -TP20 -TDS8014 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F405RG$Flash\STM32F4xx_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -158,24 +158,7 @@
           <Name>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024 -FS08000000 -FL0100000 -FP0($$Device:STM32F405RG$Flash\STM32F4xx_1024.FLM))</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
-      <Breakpoint>
-        <Bp>
-          <Number>0</Number>
-          <Type>0</Type>
-          <LineNumber>142</LineNumber>
-          <EnabledFlag>1</EnabledFlag>
-          <Address>134242516</Address>
-          <ByteObject>0</ByteObject>
-          <HtxType>0</HtxType>
-          <ManyObjects>0</ManyObjects>
-          <SizeOfObject>0</SizeOfObject>
-          <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
-          <Filename>..\Chassis\src\robot.c</Filename>
-          <ExecCommand></ExecCommand>
-          <Expression>\\template\../Chassis/src/robot.c\142</Expression>
-        </Bp>
-      </Breakpoint>
+      <Breakpoint/>
       <WatchWindow1>
         <Ww>
           <count>0</count>

diff --git a/Sensor/src/yesense.c b/Sensor/src/yesense.c
@@ -111,12 +111,12 @@ unsigned char check_data_len_by_id(Yesense* yesense, unsigned char id, unsigned
 				yesense->yaw.dot	 *= DegToRad;
 				yesense->roll.dot	 *= DegToRad;
 				yesense->pitch.dot *= DegToRad;
-				float dt						= (float) (GlobalTimer - yesense->timer) / 1000;
+				float dt						= 1000 / (float) (GlobalTimer - yesense->timer);
 				yesense->timer			= GlobalTimer;
 				if (dt != 0) {
-					yesense->roll.ddot		 = (yesense->roll.dot - yesense->roll.lastdot) / dt;
-					yesense->pitch.ddot		 = (yesense->pitch.dot - yesense->pitch.lastdot) / dt;
-					yesense->yaw.ddot			 = (yesense->yaw.dot - yesense->yaw.lastdot) / dt;
+					yesense->roll.ddot		 = (yesense->roll.dot - yesense->roll.lastdot) * dt;
+					yesense->pitch.ddot		 = (yesense->pitch.dot - yesense->pitch.lastdot) * dt;
+					yesense->yaw.ddot			 = (yesense->yaw.dot - yesense->yaw.lastdot) * dt;
 					yesense->roll.lastdot	 = yesense->roll.dot;
 					yesense->pitch.lastdot = yesense->pitch.dot;
 					yesense->yaw.lastdot	 = yesense->yaw.dot;
@@ -172,7 +172,7 @@ unsigned char check_data_len_by_id(Yesense* yesense, unsigned char id, unsigned
 				yesense->quaternion_data0 = get_signed_int(data) * NOT_MAG_DATA_FACTOR;
 				yesense->quaternion_data1 = get_signed_int(data + SINGLE_DATA_BYTES) * NOT_MAG_DATA_FACTOR;
 				yesense->quaternion_data2 = get_signed_int(data + (SINGLE_DATA_BYTES << 1)) * NOT_MAG_DATA_FACTOR;
-				yesense->quaternion_data3 = get_signed_int(data + SINGLE_DATA_BYTES * 3) * NOT_MAG_DATA_FACTOR;
+				yesense->quaternion_data3 = get_signed_int(data + +SINGLE_DATA_BYTES + (SINGLE_DATA_BYTES << 1)) * NOT_MAG_DATA_FACTOR;
 			} else
 				ret = (unsigned char) 0x00;
 		} break;

diff --git a/readme.md b/readme.md
@@ -125,6 +125,7 @@ usart1 作为通讯的接口，使用蓝牙通讯
 - 对于电机该使用的参数一定要写对，还有三角函数的计算使用的是弧度制
 - 对于一些状态量的更新，一次运行只做一次就行，多写会出问题，会造成一些较大的抖动
 - 对于腿长的控制，需要两条腿分别控制，一定不能用平均值
+- 代码运行频率要做好，也可以做一些状态的预估值来做辅助解算
 
 ### 代码运行速率的优化
 

diff --git a/流程.png b/流程.png