12.11

.clang-format

@@ -68,7 +68,7 @@ BreakConstructorInitializersBeforeComma: false
 BreakInheritanceList: AfterComma
 BreakBeforeTernaryOperators: false
 BreakStringLiterals: false
-ColumnLimit: 150
+ColumnLimit: 0
 CommentPragmas: "^ IWYU pragma:"
 CompactNamespaces: true
 ConstructorInitializerIndentWidth: 2

Chassis/inc/robot.h

@@ -11,6 +11,7 @@
 
 #pragma once
 
+#include "btoscilloscope.h"
 #include "leg.h"
 #include "yesense.h"
 typedef struct {

Chassis/src/leg.c

@@ -101,26 +101,16 @@ void Njie(Leg* leg, float xc, float yc) {
 	leg->angle1set = 2 * atan2((A + sqrt(A * A + B * B - C * C)), (B - C));
 	if (leg->angle1set < 0)
 		leg->angle1set += 2 * PI;
-
-	// nije_5(&angle1, (void *)0, x, y, l1, l6, l3, l4, l5); //利用L1,L6计算c1;
 	m							 = l1 * cos(leg->angle1set);
 	n							 = l1 * sin(leg->angle1set);
 	b							 = 0;
-	// x1 = l2 / l6 * ((x - m) * cos(b) - (y - n) * sin(b)) + m;
-	// y1 = l2 / l6 * ((x - m) * sin(b) + (y - n) * cos(b)) + n; //得到闭链五杆端点的坐标
 	x1						 = ((xc - m) * cos(b) - (yc - n) * sin(b)) + m;
 	y1						 = ((xc - m) * sin(b) + (yc - n) * cos(b)) + n;	 // 得到闭链五杆端点的坐标
 
 	A							 = 2 * y1 * l4;
 	B							 = 2 * l4 * (x1 - l5 / 2);
-	// c = l3 * l3 + 2 * l5 * x1 - l4 * l4 - l5 * l5 - x1 * x1 - y1 * y1;
 	C							 = l3 * l3 + l5 * x1 - l4 * l4 - l5 * l5 / 4 - x1 * x1 - y1 * y1;
 	leg->angle4set = 2 * atan2((A - sqrt(A * A + B * B - C * C)), (B - C));
-
-	// TODO: 根据初始角度来纠正设定角度
-	// leg->angle1set = PI - leg->angle1set - PI / 3;
-	// leg->angle4set -= PI / 3;
-	// nije_5((void *)0, &angle2, x1, y1, l1, l2, l3, l4, l5);        //计算c4 ,
 }
 
 //----
@@ -157,6 +147,6 @@ void INVMC(Leg* leg) {
 													(l4 * cos(leg->angle0.now + leg->angle2) * sin(leg->angle0.now + leg->angle3) * sin(leg->angle3 - leg->angle4) -
 													 l4 * cos(leg->angle0.now + leg->angle3) * sin(leg->angle0.now + leg->angle2) * sin(leg->angle3 - leg->angle4)) };
 
-	leg->Fnow					= trans[0][0] * leg->TFnow + trans[0][1] * leg->TBnow;
-	leg->Tpnow				= trans[1][0] * leg->TFnow + trans[1][1] * leg->TBnow;
+	leg->Fnow					= (trans[0][0] * leg->TFnow + trans[0][1] * leg->TBnow) * leg->dir;
+	leg->Tpnow				= (trans[1][0] * leg->TFnow + trans[1][1] * leg->TBnow) * leg->dir;
 }

Chassis/src/robot.c

@@ -28,10 +28,10 @@ void robotInit() {
 	robot->mode			= ROBOTNORMAL;
 
 	// TODO: 参数暂定 调节
-	pidInit(robot->L0pid, 750, 3, 1500, 0, 1000, PIDPOS);
+	pidInit(robot->L0pid, 750, 2, 1500, 0, 1000, PIDPOS);
 	pidInit(robot->yawpid, 1, 1, 1, 0, 1000, PIDPOS);
 	pidInit(robot->rollpid, 1, 1, 1, 0, 1000, PIDPOS);
-	pidInit(robot->splitpid, 1, 1, 1, 0, 1000, PIDPOS);
+	pidInit(robot->splitpid, 0.2, 0, 0, 10, 1000, PIDPOS);
 
 	robot->L0Set						= 0.35;
 	robot->yawpid->target		= 0;
@@ -71,36 +71,33 @@ void updateState() {
 void balanceMode() {
 	robot->legVir.dis.now		 = (robot->legL.dis.now + robot->legR.dis.now) / 2;
 	robot->legVir.dis.dot		 = (robot->legL.dis.dot + robot->legR.dis.dot) / 2;
-	robot->legVir.L0.now		 = robot->legR.L0.now;
-	robot->legVir.L0.dot		 = robot->legR.L0.dot;
-	robot->legVir.angle0.now = robot->legR.angle0.now;
-
-	// robot->legVir.L0.now		 = (robot->legL.L0.now + robot->legR.L0.now) / 2;
-	// robot->legVir.L0.dot		 = (robot->legL.L0.dot + robot->legR.L0.dot) / 2;
-	// robot->legVir.angle0.now = (robot->legL.angle0.now + robot->legR.angle0.now) / 2;
+	robot->legVir.L0.now		 = (robot->legL.L0.now + robot->legR.L0.now) / 2;
+	robot->legVir.L0.dot		 = (robot->legL.L0.dot + robot->legR.L0.dot) / 2;
+	robot->legVir.angle0.now = (robot->legL.angle0.now + robot->legR.angle0.now) / 2;
 	robot->legVir.angle0.dot = (robot->legL.angle0.dot + robot->legR.angle0.dot) / 2;
+
 	float L03								 = pow(robot->legVir.L0.now, 3);
 	float L02								 = pow(robot->legVir.L0.now, 2);
 	float L01								 = pow(robot->legVir.L0.now, 1);
 
-	if (robot->flyflag) {
-		for (int col = 0; col < 6; col++) {
-			for (int row = 0; row < 2; row++) {
-				int num = (col << 1) + row;
-				if (row == 1 && (col == 0 || col == 1))
-					robot->legVir.K[row][col] = Kcoeff[num][0] * L03 + Kcoeff[num][1] * L02 + Kcoeff[num][2] * L01 + Kcoeff[num][3];
-				else
-					robot->legVir.K[row][col] = 0;
-			}
-		}
-	} else {
-		for (int col = 0; col < 6; col++) {
-			for (int row = 0; row < 2; row++) {
-				int num										= (col << 1) + row;
+	//	if (robot->flyflag) {
+	for (int col = 0; col < 6; col++) {
+		for (int row = 0; row < 2; row++) {
+			int num = (col << 1) + row;
+			if (row == 1 && (col == 0 || col == 1))
 				robot->legVir.K[row][col] = Kcoeff[num][0] * L03 + Kcoeff[num][1] * L02 + Kcoeff[num][2] * L01 + Kcoeff[num][3];
-			}
+			else
+				robot->legVir.K[row][col] = 0;
 		}
 	}
+	//	} else {
+	//		for (int col = 0; col < 6; col++) {
+	//			for (int row = 0; row < 2; row++) {
+	//				int num										= (col << 1) + row;
+	//				robot->legVir.K[row][col] = Kcoeff[num][0] * L03 + Kcoeff[num][1] * L02 + Kcoeff[num][2] * L01 + Kcoeff[num][3];
+	//			}
+	//		}
+	//	}
 	robot->legVir.X.theta			= robot->legVir.angle0.now;
 	robot->legVir.X.thetadot	= robot->legVir.angle0.dot;
 	robot->legVir.X.x					= robot->legVir.dis.now;
@@ -146,18 +143,18 @@ void balanceMode() {
 	robot->legL.TWheelset -= yawCompensate;
 	robot->legR.TWheelset += yawCompensate;
 	// 劈腿补偿
-	float splitCompensate	 = 0;	 // robot->splitpid->compute(robot->splitpid, robot->legL.angle0.now - robot->legR.angle0.now);
+	float splitCompensate	 = 0;  // twiceIncCompute(robot->splitpid, robot->legL.angle0.now - robot->legR.angle0.now, robot->legL.angle0.dot - robot->legR.angle0.dot);
 	robot->legL.Tpset			-= splitCompensate;
 	robot->legR.Tpset			+= splitCompensate;
 	// 翻滚角补偿
 	float rollCompensate	 = 0;	 // robot->rollpid->compute(robot->rollpid, robot->yesense.roll.now);
-	robot->legL.Fset			+= rollCompensate;
-	robot->legR.Fset			-= rollCompensate;
+	robot->legL.Fset			-= rollCompensate;
+	robot->legR.Fset			+= rollCompensate;
 
 	VMC(&robot->legL);
 	VMC(&robot->legR);
 
-	// 方向 左侧应当-1 右侧应当1
+	// 方向
 	robot->legL.TWheelset					 *= robot->legL.dir;
 	robot->legL.TFset							 *= robot->legL.dir;
 	robot->legL.TBset							 *= robot->legL.dir;
@@ -168,6 +165,32 @@ void balanceMode() {
 
 	robot->legR.front->set.torque		= robot->legR.TFset;
 	robot->legR.behind->set.torque	= robot->legR.TBset;
+	robot->legR.wheel->set.torque		= robot->legR.TWheelset;
+
+	robot->legL.front->set.torque		= robot->legL.TFset;
+	robot->legL.behind->set.torque	= robot->legL.TBset;
+	robot->legL.wheel->set.torque		= robot->legL.TWheelset;
+
+	// float data[12];
+	// data[0]	 = robot->legR.angle0.now;
+	// data[1]	 = robot->legR.Fset;
+	// data[2]	 = robot->legR.Tpset;
+	// data[3]	 = robot->legR.TFset;
+	// data[4]	 = robot->legR.TBset;
+	// data[5]	 = robot->legR.TWheelset;
+	// data[6]	 = robot->legL.angle0.now;
+	// data[7]	 = robot->legL.Fset;
+	// data[8]	 = robot->legL.Tpset;
+	// data[9]	 = robot->legL.TFset;
+	// data[10] = robot->legL.TBset;
+	// data[11] = robot->legL.TWheelset;
+	float data[5];
+	data[0] = robot->legL.angle0.now - robot->legR.angle0.now;
+	data[1] = robot->splitpid->output;
+	data[2] = robot->legL.Tpset;
+	data[3] = robot->legR.Tpset;
+	data[4] = 0;
+	oscilloscope(data, 5);
 }
 
 //----
@@ -198,22 +221,30 @@ void haltMode() {
 void flyCheck() {
 	INVMC(&robot->legL);
 	INVMC(&robot->legR);
-	float rzwdd = robot->yesense.accelz - robot->legR.L0.ddot * cos(robot->legR.angle0.now) +
-								2.0 * robot->legR.angle0.dot * robot->legR.L0.dot * sin(robot->legR.angle0.now) +
-								robot->legR.L0.now * robot->legR.angle0.ddot * sin(robot->legR.angle0.now) +
-								robot->legR.L0.now * robot->legR.angle0.dot * robot->legR.angle0.dot * cos(robot->legR.angle0.now);
-	float lzwdd = robot->yesense.accelz - robot->legL.L0.ddot * cos(robot->legL.angle0.now) +
-								2.0 * robot->legL.angle0.dot * robot->legL.L0.dot * sin(robot->legL.angle0.now) +
-								robot->legL.L0.now * robot->legL.angle0.ddot * sin(robot->legL.angle0.now) +
-								robot->legL.L0.now * robot->legL.angle0.dot * robot->legL.angle0.dot * cos(robot->legL.angle0.now);
-
-	robot->legR.normalforce = MASSWHEEL * rzwdd + GRAVITY * MASSWHEEL + robot->legR.Fnow * cos(robot->legR.angle0.now) +
-														robot->legR.TWheelnow * sin(robot->legR.angle0.now) / robot->legR.L0.now;
-	robot->legL.normalforce = MASSWHEEL * lzwdd + GRAVITY * MASSWHEEL + robot->legL.Fnow * cos(robot->legL.angle0.now) +
-														robot->legL.TWheelnow * sin(robot->legL.angle0.now) / robot->legL.L0.now;
-	float force = (robot->legL.normalforce + robot->legR.normalforce) / 2;
-
-	if (force > FORCETHRESHOLD)
+	float lp		= robot->legL.Fnow * cos(robot->legL.angle0.now) + robot->legL.Tpnow * sin(robot->legL.angle0.now) / robot->legL.L0.now;
+	float rp		= robot->legR.Fnow * cos(robot->legR.angle0.now) + robot->legR.Tpnow * sin(robot->legR.angle0.now) / robot->legR.L0.now;
+
+	float zmdd	= robot->yesense.accelz * cos(robot->yesense.pitch.now) - robot->yesense.accelx * sin(robot->yesense.pitch.now);
+
+	float lzwdd = zmdd - robot->legL.L0.ddot * cos(robot->legL.angle0.now) + 2 * robot->legL.L0.dot + robot->legL.angle0.dot * sin(robot->legL.angle0.now) + robot->legL.L0.now * robot->legL.angle0.ddot * sin(robot->legL.angle0.now) + robot->legL.L0.now * robot->legL.angle0.dot * robot->legL.angle0.dot * cos(robot->legL.angle0.now);
+	float rzwdd = zmdd - robot->legR.L0.ddot * cos(robot->legR.angle0.now) + 2 * robot->legR.L0.dot + robot->legR.angle0.dot * sin(robot->legR.angle0.now) + robot->legR.L0.now * robot->legR.angle0.ddot * sin(robot->legR.angle0.now) + robot->legR.L0.now * robot->legR.angle0.dot * robot->legR.angle0.dot * cos(robot->legR.angle0.now);
+	;
+
+	// float rzwdd = robot->yesense.accelz - robot->legR.L0.ddot * cos(robot->legR.angle0.now) +
+	// 							2.0 * robot->legR.angle0.dot * robot->legR.L0.dot * sin(robot->legR.angle0.now) +
+	// 							robot->legR.L0.now * robot->legR.angle0.ddot * sin(robot->legR.angle0.now) +
+	// 							robot->legR.L0.now * robot->legR.angle0.dot * robot->legR.angle0.dot * cos(robot->legR.angle0.now);
+	// float lzwdd = robot->yesense.accelz - robot->legL.L0.ddot * cos(robot->legL.angle0.now) +
+	// 							2.0 * robot->legL.angle0.dot * robot->legL.L0.dot * sin(robot->legL.angle0.now) +
+	// 							robot->legL.L0.now * robot->legL.angle0.ddot * sin(robot->legL.angle0.now) +
+	// 							robot->legL.L0.now * robot->legL.angle0.dot * robot->legL.angle0.dot * cos(robot->legL.angle0.now);
+
+	robot->legR.normalforce = rp + MASSWHEEL * (GRAVITY + rzwdd);
+	robot->legL.normalforce = lp + MASSWHEEL * (GRAVITY + lzwdd);
+
+	float force							= (robot->legL.normalforce + robot->legR.normalforce) / 2;
+
+	if (force < 20)
 		robot->flyflag = true;
 	else
 		robot->flyflag = false;

Chassis/src/robotparam.c

@@ -18,9 +18,9 @@ void outputInit(Output* output) {
 vector2f jumpPoints[4] = {
 
 };
-float Kcoeff[12][4] = { 17.01888975,	-0.903154769, -31.57238975, -41.9788821,	85.78235496, -49.52948422, -40.63369617, 40.35634291,
-												42.03139839,	-53.61799535, 13.57488613,	-13.64542004, 18.23634353, -7.239930703, -12.5439447,	 11.26342392,
-												-2.884425269, 6.643548023,	-5.128265678, -8.547027497, 9.049735285, 4.590669416,	 -17.93157685, 10.55885787,
-												10.96680271,	-13.28880131, 4.268582383,	-11.41664036, 5.762044557, 11.65149503,	 -23.13892639, 12.2324679,
-												46.51992207,	-6.127866058, -50.17355563, 33.52934316,	30.79277867, -76.25101817, 61.17595912,	 123.0752698,
-												-4.773544463, 9.723701106,	-7.737713765, 2.793511935,	3.104944241, -6.038839679, 4.125493328,	 4.549881246 };
+float Kcoeff[12][4] = { 5.630528576, 15.05094608, -40.900907, -39.1855192, 28.1266928, 6.568470187, -58.6139567, 44.68225365,
+												39.53508614, -50.01718642, 11.45191634, -13.19387547, -5.300198438, 19.31860753, -23.65277373, 13.67528826,
+												-2.945370449, 6.369792673, -4.904018795, -8.573465244, 0.741957965, 11.51517788, -19.40172617, 10.69760755,
+												11.75474104, -14.86230995, 5.28364609, -11.72307273, -7.008873702, 24.88037826, -28.07447064, 13.20849821,
+												-11.26845646, 62.06813618, -81.23280198, 41.49013406, 52.70157827, -109.0562871, 81.35334175, 118.8723209,
+												-7.913680955, 13.0993646, -8.836252186, 3.19453246, 4.612785565, -8.366286295, 5.677434103, 4.025845438 };

Function/inc/btoscilloscope.h

@@ -0,0 +1,14 @@
+//----
+// @file btoscilloscope.h
+// @author mask <[email protected]>
+// @brief 蓝牙示波器——基于蓝牙调试器
+// @version 1.0
+// @date 2023-12-07
+//
+// @copyright Copyright (c) 2023
+//
+//----
+#include "string.h"
+#include "usart.h"
+
+void oscilloscope(float* data, u8 size);

Function/inc/pid.h

@@ -42,4 +42,6 @@ float posCompute(PID* pid, float input);
 float TincCompute(PID* pid, float input);
 float TposCompute(PID* pid, float input);
 
+float twiceIncCompute(PID* pid, float input, float inputdot);
+
 void pidInit(PID* pid, float kp, float ki, float kd, float outputLimit, float accErrLimit, pidMode mode);

Function/inc/vofa.h

@@ -12,4 +12,4 @@
 #include "mymath.h"
 #include "usart.h"
 
-void Vofa_Send_JustFloat(float data1, float data2, float data3, float data4, float data5);
+void vofaJustFloat(float data1, float data2, float data3, float data4, float data5);

Function/src/btoscilloscope.c

@@ -0,0 +1,19 @@
+#include "btoscilloscope.h"
+
+#define HEADCHAR 0xA5
+#define TAILCHAR 0x5A
+
+void oscilloscope(float* data, u8 size) {
+	u8 txmsg[67];	 // 包头包尾校验位 + 数据，最多16个float，但是一般不那么多
+	txmsg[0] = HEADCHAR;
+	int sz	 = (size << 2) + 1;
+	memcpy(&txmsg[1], data, 4 * size);
+	int sum = 0;
+	for (int i = 1; i < sz; i++) 
+  {
+    sum += txmsg[i];
+  }
+	txmsg[sz++] = sum & 0xff;
+	txmsg[sz++] = TAILCHAR;
+	usart1->send(txmsg, sz);
+}

Function/src/pid.c

@@ -68,6 +68,27 @@ float TposCompute(PID* pid, float input) {
 	return pid->output;
 }
 
+//----
+// @brief 二次控制，同时控制位置和速度
+//
+// @param pid
+// @param input
+// @param inputdot
+// @return float
+//----
+float twiceIncCompute(PID* pid, float input, float inputdot) {
+	pid->err[0]	 = pid->target - input;
+	float errdot = 0 - inputdot;
+	pid->output	 = pid->kp * pid->err[0] + pid->kd * errdot;
+	if (pid->ki) {
+		pid->accErr += inputdot;
+		limitInRange(float)(&pid->accErr, pid->accErrLimit);
+		pid->output += pid->ki * pid->accErr;
+	}
+	limitInRange(float)(&pid->output, pid->outputLimit);
+	return pid->output;
+}
+
 //----
 // @brief PID初始化，设置kp,ki,kd 选定pid模式
 //

Function/src/vofa.c

@@ -8,7 +8,7 @@ void FloatTo4Byte(float* f, u8* buff) {
 	*(++buff) = p_float[2];
 	*(++buff) = p_float[3];
 }
-void Vofa_Send_JustFloat(float data1, float data2, float data3, float data4, float data5) {
+void vofaJustFloat(float data1, float data2, float data3, float data4, float data5) {
 	u8 SendData[24] = { 0 };
 	FloatTo4Byte(&data1, &SendData[0]);
 	FloatTo4Byte(&data2, &SendData[4]);

Hardware/inc/usart.h

@@ -21,7 +21,7 @@
 #include "stm32f4xx_usart.h"
 #include "string.h"
 
-#define MAXUSARTDATASIZE 50
+#define MAXUSARTDATASIZE 70
 
 typedef struct Usart {
 	u8 rxBuff[MAXUSARTDATASIZE];

Hardware/src/can.c

@@ -141,7 +141,7 @@ void canSend(u8 ctrlWord) {
 	if ((ctrlWord & 0x01) && can1Txmsg->pop(can1Txmsg, &msg))
 		CAN_Transmit(CAN1, &msg);
 	if ((ctrlWord & 0x02) && can2Txmsg->pop(can2Txmsg, &msg))
-		CAN_Transmit(CAN2, &msg);
+ 		CAN_Transmit(CAN2, &msg);
 }
 
 void canCheck() { }

Hardware/src/led.c

@@ -36,4 +36,4 @@ void ledShow() {
 	LED_GREEN_ON;
 	OSTimeDly(5000);
 	LED_BLUE_OFF;
-}
+}

Main/src/irqhandler.c

@@ -56,10 +56,11 @@ void TIM3_IRQHandler(void) {
 		TmotorMonitor(tmotor);
 		RobotStateUpdate(&robotstate);
 
-		Vofa_Send_JustFloat((float) robot->legR.Fset, (float) robot->L0pid->output, (float) robot->legR.L0.now, (float) robot->L0Set,
-												(float) robot->legR.Fnow);
-		// Vofa_Send_JustFloat((float) robot->legL.Fset, (float) robot->L0pid->output, (float) robot->legL.L0.now, (float) robot->L0Set, 0);
-		// Vofa_Send_JustFloat();
+		// vofaJustFloat((float) robot->legR.Fset, (float) robot->L0pid->output, (float) robot->legR.L0.now, (float) robot->L0Set,
+		// 										(float) robot->legR.Fnow);
+		// vofaJustFloat((float) robot->legL.Fset, (float) robot->L0pid->output, (float) robot->legL.L0.now, (float) robot->L0Set, 0);
+
+		// vofaJustFloat();
 		// blueToothSend(3, (void*) &robotstate, sizeof(RobotState));
 	}
 	TIM_ClearITPendingBit(TIM3, TIM_IT_Update);

Main/src/main.c

@@ -31,7 +31,7 @@ static void taskStart(void* pdata) {
 
 	OSTaskCreate(taskLed, (void*) 0, &taskLedStk[TASK_STK_SIZE - 1], LED_TASK_PRIO);
 	OSTaskCreate(taskBeep, (void*) 0, &taskBeepStk[TASK_STK_SIZE - 1], BEEP_TASK_PRIO);
-	OSTaskCreate(taskRun, (void*) 0, &taskRunStk[TASK_STK_SIZE - 1], RUN_TASK_PRIO);
+  OSTaskCreate(taskRun, (void*) 0, &taskRunStk[TASK_STK_SIZE - 1], RUN_TASK_PRIO);
 	// OSTaskCreate(taskTest, (void*) 0, &taskTestStk[TASK_STK_SIZE - 1], TEST_TASK_PRIO);
 
 	OS_EXIT_CRITICAL();							 // 程序退出临界段，可以被中断打断，在临界段中不要加延时，会死机

Motor/inc/motorparam.h

@@ -18,7 +18,10 @@
 // @brief 电机模式
 //
 //----
-typedef enum { HALT, POSITION, SPEED, TORQUE } MotorMode;
+typedef enum { HALT,
+							 POSITION,
+							 SPEED,
+							 TORQUE } MotorMode;
 
 //----
 // @brief 监测电机状态
@@ -42,7 +45,7 @@ typedef struct {
 	typedef struct {          \
 		T			current;          \
 		T			velocity;         \
-		T			angleRad;         \
+		float angleRad;         \
 		float angleDeg;         \
 		float torque;           \
 	} MotorValue_##T;