11.12

.gitignore

@@ -0,0 +1 @@
+.vscode

Function/inc/datastruct.h

@@ -0,0 +1,23 @@
+//----
+// @file datastruct.h
+// @author mask ([email protected])
+// @brief 
+// @version 1.0
+// @date 2023-11-12
+// 
+// @copyright Copyright (c) 2023
+// @description this is a datastruct to storage some wheelfoot data
+//----
+#pragma once
+
+typedef struct {
+  float now;
+  float dot;
+  float ddot;
+  float last;
+  float lastdot;
+  float set;
+  float setdot;
+} datastruct;
+
+void datastructInit(datastruct* data, const float _now, const float _last, const float _dot, const float _ddot);

Function/inc/mymath.h

@@ -0,0 +1,34 @@
+//----
+// @file mymath.h
+// @author mask ([email protected])
+// @brief 
+// @version 1.0
+// @date 2023-11-12
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+
+#pragma once
+#include "stm32f4xx.h"
+#include "string.h"
+#include "arm_math.h"
+
+#define AngleToRad 0.0174533f
+#define RadToAngle 57.295780f
+#define cos(x) arm_cos_f32(x)
+#define sin(x) arm_sin_f32(x)
+#define sqrt(x) arm_sqrt_f32(x)
+#define abs(x) arm_abs_f32(x)
+#define square(x) ((x) * (x));
+#define max(x, y) (((x) > (y)) ? (x) : (y))
+#define min(x, y) (((x) > (y)) ? (y) : (x))
+#define halfPI 1.57079633f
+
+void S16ToU8 (s16* s, u8* u); 
+void U8ToS16 (u8* u, s16* s);
+void F32ToU8 (float* f, u8* u);
+void U8ToF32 (u8* u, float* f);
+
+void limitInRange(float* val, float* limit);
+void limitIn2Range(float* val, float* min, float* max);

Function/inc/queue.h

@@ -0,0 +1,102 @@
+//----
+// @file queue.h
+// @author mask ([email protected])
+// @brief 
+// @version 1.0
+// @date 2023-11-11
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+#pragma once
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+
+#define Queue(T) typedef struct {\
+  T *val;\
+  int head;\
+  int tail;\
+  int size;\
+  int (*maxsize)(struct queue_##T* queue);\
+  int (*getsize)(struct queue_##T* queue);\
+  int (*isempty)(struct queue_##T* queue);\
+  int (*isfull)(struct queue_##T* queue);\
+  void (*push)(struct queue_##T* queue, T val);\
+  void (*pop)(struct queue_##T* queue);\
+  T (*top)(struct queue_##T* queue);\
+  void (*clear)(struct queue_##T *queue);\
+} queue_##T;
+#define queue(T) queue_##T
+#define Maxsize(T) int maxsize_##T(queue_##T *queue){return queue->size;}
+#define Getsize(T) int getsize_##T(queue_##T *queue){\
+  int size = queue->tail - queue->head;\
+  while(size < 0)size += queue->size;\
+  while(size > queue->size)size -= queue->size;\
+  return size;\
+}
+#define Isempty(T) int isempty_##T(queue_##T *queue){\
+  return queue->head == queue->tail;\
+}
+#define Isfull(T) int isfull_##T(queue_##T *queue){\
+  return queue->tail == (queue->head - 1 >= 0? queue->head - 1 : queue->head - 1 + queue->size);\
+}
+#define Push(T) void push_##T(queue_##T *queue, T val){\
+  if(queue->tail == (queue->head - 1 >= 0? queue->head - 1 : queue->head - 1 + queue->size))return;\
+  queue->val[queue->tail] = val;\
+  queue->tail += 1;\
+  queue->tail %= queue->size;\
+}
+#define Pop(T) void pop_##T(queue_##T *queue){\
+  if(queue->head == queue->tail){\
+    printf("this queue is empty");\
+    return;\
+  }\
+  queue->head++;\
+  queue->head %= queue->size;\
+}
+#define Top(T) T top_##T(queue_##T *queue){\
+  if(queue->head == queue->tail){\
+    printf("this queue is empty");\
+    return (T)0;\
+  }\
+  return queue->val[queue->head];\
+}
+#define Clear(T) void clear_##T(queue_##T *queue){\
+  queue->head = 0;\
+  queue->tail = 0;\
+}
+#define NewQueue(T) queue_##T *newqueue_##T(int size){\
+  queue_##T *queue = (queue_##T *) malloc(sizeof(queue_##T));\
+  queue->val = (T *) malloc(sizeof(T) * size);\
+  queue->size = size;\
+  queue->head = 0;\
+  queue->tail = 0;\
+  queue->maxsize = maxsize_##T;\
+  queue->getsize = getsize_##T;\
+  queue->isempty = isempty_##T;\
+  queue->isfull = isfull_##T;\
+  queue->push = push_##T;\
+  queue->pop = pop_##T;\
+  queue->top = top_##T;\
+  queue->clear = clear_##T;\
+  return queue;\
+}
+#define newqueue(T) newqueue_##T
+#define DeleteQueue(T) void deletequeue_##T(queue_##T* queue){\
+  free(queue->val);\
+  free(queue);\
+}
+#define deletequeue(T) deletequeue_##T
+#define DEFINEQUEUE(T) Queue(T)\
+  Maxsize(T)\
+  Getsize(T)\
+  Isempty(T)\
+  Isfull(T)\
+  Push(T)\
+  Pop(T)\
+  Top(T)\
+  Clear(T)\
+  NewQueue(T)\
+  DeleteQueue(T)

Function/inc/vector.h

@@ -0,0 +1,35 @@
+//----
+// @file vector.h
+// @author mask ([email protected])
+// @brief 
+// @version 1.0
+// @date 2023-11-11
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+#pragma once
+
+typedef struct {
+  float x;
+  float y;
+} vector2f;
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+} vector3f;
+
+vector2f vector2fAdd(const vector2f vec1, const vector2f vec2);
+vector2f vector2fSub(const vector2f vec1, const vector2f vec2);
+vector2f vector2fMulty(const vector2f vec1, const float num);
+vector2f vector2fDivid(const vector2f vec1, const float num);
+float vector2fDot(const vector2f vec1, const vector2f vec2);
+
+vector3f vector3fAdd(const vector3f vec1, const vector3f vec2);
+vector3f vector3fSub(const vector3f vec1, const vector3f vec2);
+vector3f vector3fMulty(const vector3f vec1, const float num);
+vector3f vector3fDivid(const vector3f vec1, const float num);
+float vector3fDot(const vector3f vec1, const vector3f vec2);
+

Function/src/datastruct.c

@@ -0,0 +1,8 @@
+#include "datastruct.h"
+
+void datastructInit(datastruct* data, const float _now, const float _last, const float _dot, const float _ddot) {
+  data->now = _now;
+  data->last = _now;
+  data->dot = _now;
+  data->ddot = _now;
+}

Function/src/mymath.c

@@ -0,0 +1,26 @@
+#include "mymath.h"
+
+void S16ToU8 (s16* s, u8* u) {
+  memcpy(u, s, sizeof(s16));
+}
+
+void U8ToS16 (u8* u, s16* s) {
+  memcpy(s, u, sizeof(s16));
+}
+void F32ToU8 (float* f, u8* u) {
+  memcpy(u, f, sizeof(float));
+}
+
+void U8ToF32 (u8* u, float* f) {
+  memcpy(f, u, sizeof(float));
+}
+
+void limitInRange(float* val, float* limit) {
+  if(*val < -*limit) *val = -*limit;
+  else if(*val > *limit) *val = *limit;
+}
+
+void limitIn2Range(float* val, float* min, float* max) {
+  if(*val < *min) *val = *min;
+  else if(*val > *max) *val = *max;
+}

Function/src/vector.c

@@ -0,0 +1,72 @@
+#include "vector.h"
+
+vector2f vector2fAdd (const vector2f vec1, const vector2f vec2) {
+  vector2f vec = {
+    .x = vec1.x + vec2.x,
+    .y = vec1.y + vec2.y,
+  };
+  return vec;
+}
+vector2f vector2fSub (const vector2f vec1, const vector2f vec2) {
+  vector2f vec = {
+    .x = vec1.x - vec2.x,
+    .y = vec1.y - vec2.y,
+  };
+  return vec;
+
+}
+vector2f vector2fMulty (const vector2f vec1, const float num) {
+  vector2f vec = {
+    .x = vec1.x * num,
+    .y = vec1.y * num,
+  };
+  return vec;
+}
+vector2f vector2fDivid (const vector2f vec1, const float num) {
+  if (num == 0) return vec1;
+  vector2f vec = {
+    .x = vec1.x / num,
+    .y = vec1.y / num,
+  };
+  return vec;
+}
+float vector2fDot (const vector2f vec1, const vector2f vec2) {
+  return vec1.x * vec2.x + vec1.y * vec2.y;
+}
+
+vector3f vector3fAdd (const vector3f vec1, const vector3f vec2) {
+  vector3f vec = {
+    .x = vec1.x + vec2.x,
+    .y = vec1.y + vec2.y,
+    .z = vec1.z + vec2.z,
+  };
+  return vec;
+}
+vector3f vector3fSub (const vector3f vec1, const vector3f vec2) {
+  vector3f vec = {
+    .x = vec1.x - vec2.x,
+    .y = vec1.y - vec2.y,
+    .z = vec1.z - vec2.z,
+  };
+  return vec;
+}
+vector3f vector3fMulty (const vector3f vec1, const float num) {
+  vector3f vec = {
+    .x = vec1.x * num,
+    .y = vec1.y * num,
+    .z = vec1.z * num,
+  };
+  return vec;
+}
+vector3f vector3fDivid (const vector3f vec1, const float num) {
+  if (num == 0) return vec1;
+  vector3f vec = {
+    .x = vec1.x / num,
+    .y = vec1.y / num,
+    .z = vec1.z / num,
+  };
+  return vec;
+}
+float vector3fDot (const vector3f vec1, const vector3f vec2) {
+  return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
+}

Hardware/inc/beep.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "os_cfg.h"
+#include "os_cpu.h"
+#include "stm32f4xx.h"
+#include "stm32f4xx_gpio.h"
+#include "stm32f4xx_rcc.h"
+#include "ucos_ii.h"
+
+#define BEEP_OFF GPIOA->BSRRH = GPIO_Pin_8
+#define BEEP_ON	 GPIOA->BSRRL = GPIO_Pin_8
+
+void beepInit();
+void beepShow(int num);
+void beepWarning();

Hardware/inc/can.h

@@ -0,0 +1,29 @@
+//----
+// @file can.h
+// @author mask ([email protected])
+// @brief 
+// @version 1.0
+// @date 2023-11-12
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+
+//! RTR一定得设置，不然会发生id错误的情况，并且数据不对
+#pragma once
+
+#include "misc.h"
+#include "queue.h"
+#include "stm32f4xx.h"
+#include "stm32f4xx_can.h"
+#include "stm32f4xx_gpio.h"
+#include "stm32f4xx_rcc.h"
+
+void can1Init();
+void can2Init();
+
+void can1Check();
+void can2Check();
+
+void canSend(u8 ctrlWord);
+void sendzero();

Hardware/inc/delay.h

@@ -0,0 +1,18 @@
+//----
+// @file delay.h
+// @author mask ([email protected])
+// @brief 
+// @version 1.0
+// @date 2023-11-12
+// 
+// @copyright Copyright (c) 2023
+// 
+//----
+
+#pragma once
+
+#include "stm32f4xx.h"
+
+void delay_ms(const unsigned int t);
+void delay_us(const unsigned int t);
+void delay(u16 t);