11.13

Chassis/inc/leg.h

@@ -0,0 +1,22 @@
+//----
+// @file leg.h
+// @author mask <[email protected]>
+// @brief 
+// @version 1.0
+// @date 2023-11-12
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+
+#pragma once
+
+#include "djmotor.h"
+#include "tmotor.h"
+
+
+
+typedef struct {
+  DJmotor* wheel;
+  Tmotor* front, behind;
+}Leg;

Chassis/inc/robot.h

@@ -0,0 +1,16 @@
+//----
+// @file robot.h
+// @author mask <[email protected]>
+// @brief 
+// @version 1.0
+// @date 2023-11-12
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+
+#pragma once
+
+typedef struct {
+
+} Robot;

Chassis/inc/robotparam.h

@@ -0,0 +1,42 @@
+//----
+// @file robotparam.h
+// @author mask <[email protected]>
+// @brief 
+// @version 1.0
+// @date 2023-11-13
+// 
+// @copyright Copyright (c) 2023
+// @details 机器人所需要的所有参数的定义，但是不完全需要测定
+//----
+#pragma once
+
+#define L1 0.18f
+#define L2 0.336f
+#define L3 0.336f
+#define L4 0.18f
+#define L5 0.24f
+
+// TODO: 重量测定和参数计算
+#define MASSL1 
+#define MASSL2 
+#define MASSL3 
+#define MASSL4 
+#define MASSWHEEL
+#define MASSBODY
+#define GRAVITY 9.805f
+
+float Kcoeff[12][4] = {0};
+
+typedef struct {
+  float theta;
+  float thetadot;
+  float x;
+  float v;
+  float pitch;
+  float pitchdot;
+}Input;
+
+typedef struct {
+  float Tp;
+  float Twheel;
+}Output;

Main/inc/irqhandler.h

@@ -0,0 +1,28 @@
+//----
+// @file irqhandler.h
+// @author mask <[email protected]>
+// @brief 
+// @version 1.0
+// @date 2023-11-13
+// 
+// @copyright Copyright (c) 2023
+// @details 对于一些消息接收中断的处理
+//----
+#pragma once
+
+#include "can.h"
+#include "tim.h"
+#include "usart.h"
+#include "yesense.h"
+
+void DMA2_Stream7_IRQHandler();
+void DMA1_Stream6_IRQHandler();
+
+void USART1_IRQHandler();
+void USART2_IRQHandler();
+
+void TIM2_IRQHandler();
+void TIM3_IRQHandler();
+
+void CAN1_RX0_IRQHandler();
+void CAN2_RX0_IRQHandler();

Main/src/irqhandler.c

@@ -0,0 +1,126 @@
+#include "irqhandler.h"
+
+u8	  HEAD_FLAG		   = 0;	   // 包头标志位
+u8	  RX_FLAG		   = 0;
+short Data_len		   = 2;
+u8	  Data_Yesense[98] = {0x59, 0x53};
+int	  res			   = 0;
+u8	  n;
+float yaw_last = 0, yaw_now = 0;
+
+void  G_output_infoSet(Yesense* yesense)
+{
+	yaw_last = yaw_now;
+	yaw_now	 = icm.yawAngle;
+	if (yaw_now - yaw_last < -100)	  // 发生了突变
+		n += 1;
+	else if (yaw_now - yaw_last > 100)
+		n -= 1;
+	icm.yawAngle += n * 360;
+}
+
+void DMA2_Stream7_IRQHandler(void)	  // 数据传输完成，产生中断，检查是否还有没有传输的数据，继续传输
+{
+	if (DMA_GetITStatus(DMA2_Stream7, DMA_IT_TCIF7) == SET) {
+		DMA_ClearFlag(DMA2_Stream7, DMA_IT_TCIF7);	  // 清除中断标志
+		DMA_ClearITPendingBit(DMA2_Stream7, DMA_IT_TCIF7);
+	}
+}
+
+void DMA1_Stream6_IRQHandler(void)	  // 数据传输完成,产生中断,检查是否还有没有传输的数据，继续传输
+{
+	if (DMA_GetITStatus(DMA1_Stream6, DMA_IT_TCIF6) == SET) {
+		DMA_ClearFlag(DMA1_Stream6, DMA_IT_TCIF6);	  // 清除中断标志
+		DMA_ClearITPendingBit(DMA1_Stream6, DMA_IT_TCIF6);
+	}
+}
+
+void USART1_IRQHandler(void)
+{
+	u8 temp;
+	if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)	  // 接收寄存器非空
+	{
+		USART_ClearFlag(USART1, USART_IT_RXNE);	   // USART_FLAG_RXNE        //清除中断标志
+		USART_ClearITPendingBit(USART1, USART_IT_RXNE);
+		temp = USART_ReceiveData(USART1);
+		// receiveData(controlMsg, temp);
+		// controlMsg->receiveData(controlMsg, temp);
+	}
+}
+
+void USART2_IRQHandler(void)
+{
+	u8 temp;
+
+	if (USART_GetITStatus(USART2, USART_IT_ORE_RX) != RESET) {
+		temp = USART_ReceiveData(USART2);
+		USART_ClearFlag(USART2, USART_FLAG_ORE);	// 清除溢出中断
+	}
+	if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
+		USART_ClearFlag(USART2, USART_FLAG_RXNE);
+		USART_ClearITPendingBit(USART2, USART_IT_RXNE);
+
+		temp = USART_ReceiveData(USART2);
+		if (RX_FLAG == 1) {
+			Data_len++;
+			Data_Yesense[Data_len - 1] = temp;
+			if (Data_len > 97)	  // 超出范围
+			{
+				Data_len = 2;
+				RX_FLAG	 = 0;
+			}
+		}
+		if (HEAD_FLAG == 1) {
+			if (temp == 0x53)	 // 帧头2
+			{
+				Data_len  -= 2;
+				res		   = analysis_data(Data_Yesense, Data_len);
+				Data_len   = 2;
+				RX_FLAG	   = 1;
+				HEAD_FLAG  = 0;
+				if (res == 0 || res == 1)
+					G_output_infoSet();
+			} else if (temp != 0x59)
+				HEAD_FLAG = 0;
+		}
+		if (temp == 0x59)	 // 帧头1
+			HEAD_FLAG = 1;
+	}
+}
+
+void TIM2_IRQHandler(void)	  // 1ms
+{
+	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)	// 溢出中断
+	{
+
+	}
+	TIM_ClearITPendingBit(TIM2, TIM_IT_Update);	   // 清除中断标志位
+}
+
+void TIM3_IRQHandler(void)	  // 100ms
+{
+	if (TIM_GetITStatus(TIM3, TIM_IT_Update) == SET) {
+
+	}
+	TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
+}
+
+void CAN1_RX0_IRQHandler(void)
+{
+	CAN_ClearITPendingBit(CAN1, CAN_IT_FMP0);
+	CAN_ClearFlag(CAN1, CAN_IT_FMP0);
+	if (CAN_GetITStatus(CAN1, CAN_IT_FMP0) != RESET) {
+		CanRxMsg rxMsg;
+		CAN_Receive(CAN1, CAN_FIFO0, &rxMsg);
+	}
+}
+
+void CAN2_RX0_IRQHandler(void)
+{
+	CAN_ClearITPendingBit(CAN2, CAN_IT_FMP0);
+	CAN_ClearFlag(CAN2, CAN_IT_FMP0);
+	if (CAN_GetITStatus(CAN2, CAN_IT_FMP0) != RESET) {
+		CanRxMsg rxMsg;
+		CAN_Receive(CAN2, CAN_FIFO0, &rxMsg);
+	}
+}

Sensor/inc/yesense.h

@@ -0,0 +1,44 @@
+//----
+// @file yesense.h
+// @author mask <[email protected]>
+// @brief 
+// @version 1.0
+// @date 2023-11-13
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+#pragma once
+
+#include "datastruct.h"
+#include <stdbool.h>
+
+typedef struct {
+	float accelx; // m/s2
+	float accely;
+	float accelz;
+
+	float magx;
+	float magy;
+	float magz;
+
+	float rawMagx;		/*uinit: mGauss*/
+	float rawMagy;
+	float rawMagz;
+
+  datastruct pitch; // deg
+  datastruct roll;
+  datastruct yaw;
+
+	float quaternion_data0;
+	float quaternion_data1;	
+	float quaternion_data2;
+	float quaternion_data3;
+
+  unsigned char data[98];
+  short datalen;
+  bool init;
+}Yesense;
+
+int yesenseAnalyze(Yesense* yesense, unsigned char *data, short len);
+