RoboDuoduo/r1upper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

基础动作

Browse Source
This commit is contained in:
ws 2025-05-21 13:17:25 +08:00
parent 95ea791d34
commit 3d482325fb
16 changed files with 420 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Src/gpio.c
View File

@ -76,7 +76,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = up_ball_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(up_ball_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
@ -121,7 +121,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = STOP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(STOP_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */

36
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -54,3 +54,39 @@
[info] Log at : 2025/4/26|16:18:23|GMT+0800
[info] Log at : 2025/5/7|20:43:43|GMT+0800
[info] Log at : 2025/5/8|16:11:00|GMT+0800
[info] Log at : 2025/5/10|16:53:30|GMT+0800
[info] Log at : 2025/5/10|20:53:12|GMT+0800
[info] Log at : 2025/5/11|14:55:09|GMT+0800
[info] Log at : 2025/5/11|15:23:30|GMT+0800
[info] Log at : 2025/5/12|19:40:58|GMT+0800
[info] Log at : 2025/5/13|20:50:37|GMT+0800
[info] Log at : 2025/5/14|20:21:54|GMT+0800
[info] Log at : 2025/5/15|14:31:29|GMT+0800
[info] Log at : 2025/5/15|19:24:56|GMT+0800
[info] Log at : 2025/5/17|21:50:24|GMT+0800
[info] Log at : 2025/5/18|16:09:49|GMT+0800
[info] Log at : 2025/5/18|19:22:14|GMT+0800
[info] Log at : 2025/5/18|23:12:00|GMT+0800
[info] Log at : 2025/5/19|21:08:52|GMT+0800
[info] Log at : 2025/5/20|00:19:48|GMT+0800
[info] Log at : 2025/5/20|19:15:21|GMT+0800

4
MDK-ARM/.vscode/settings.json vendored
View File

@ -11,6 +11,8 @@
"nuc.h": "c",
"crc_ccitt.h": "c",
"functional": "cpp",
"vofa.h": "c"
"vofa.h": "c",
"user_math.h": "c",
"queue": "cpp"
}
}

4
MDK-ARM/.vscode/uv4.log vendored
View File

@ -2,7 +2,7 @@
Build target 'R1-shooter'
compiling ball.cpp...
linking...
Program Size: Code=26644 RO-data=1812 RW-data=240 ZI-data=23520
Program Size: Code=28460 RO-data=1824 RW-data=256 ZI-data=23936
FromELF: creating hex file...
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:03
Build Time Elapsed: 00:00:02

2
MDK-ARM/.vscode/uv4.log.lock vendored
View File

@ -1 +1 @@
2025/4/26 16:32:18
2025/5/20 23:55:49

90
MDK-ARM/R1-shooter.uvoptx
View File

@ -183,77 +183,77 @@
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>angle1,0x0A</ItemText>
<ItemText>cnt1,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>k,0x0A</ItemText>
<ItemText>cmd_fromnuc</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cnt1,0x0A</ItemText>
<ItemText>nucbuf</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText>
<ItemText>nucData</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText>
<ItemText>send,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>nucData</ItemText>
<ItemText>\\R1_shooter\../User/task/ballTask.cpp\ball,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>send,0x0A</ItemText>
<ItemText>abc,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>speed1,0x0A</ItemText>
<ItemText>ball_state,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>\\R1_shooter\../User/task/ballTask.cpp\ball,0x0A</ItemText>
<ItemText>flag,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>currentState1</ItemText>
<ItemText>shoot,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>abc,0x0A</ItemText>
<ItemText>speedm,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>ball_state,0x0A</ItemText>
<ItemText>a1,0x0A</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>flag,0x0A</ItemText>
<ItemText>a2,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>triggerCount,0x0A</ItemText>
<ItemText>shoot_flag,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>last_ball_state,0x0A</ItemText>
<ItemText>angle1,0x0A</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
@ -957,6 +957,30 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>50</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\lib\filter.c</PathWithFileName>
<FilenameWithoutPath>filter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>51</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\lib\kalman.c</PathWithFileName>
<FilenameWithoutPath>kalman.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -967,7 +991,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>50</FileNumber>
<FileNumber>52</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -979,7 +1003,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>51</FileNumber>
<FileNumber>53</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -991,7 +1015,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>52</FileNumber>
<FileNumber>54</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1003,7 +1027,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>53</FileNumber>
<FileNumber>55</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1015,7 +1039,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>54</FileNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1027,7 +1051,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>55</FileNumber>
<FileNumber>57</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1041,13 +1065,13 @@
<Group>
<GroupName>User/module</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>56</FileNumber>
<FileNumber>58</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1059,7 +1083,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>57</FileNumber>
<FileNumber>59</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1071,7 +1095,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>58</FileNumber>
<FileNumber>60</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1083,7 +1107,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>59</FileNumber>
<FileNumber>61</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1103,7 +1127,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>60</FileNumber>
<FileNumber>62</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1115,7 +1139,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>61</FileNumber>
<FileNumber>63</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1127,7 +1151,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>62</FileNumber>
<FileNumber>64</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1139,7 +1163,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>63</FileNumber>
<FileNumber>65</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1151,7 +1175,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>64</FileNumber>
<FileNumber>66</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1163,7 +1187,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>65</FileNumber>
<FileNumber>67</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1175,7 +1199,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>66</FileNumber>
<FileNumber>68</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

10
MDK-ARM/R1-shooter.uvprojx
View File

@ -658,6 +658,16 @@
<FileType>1</FileType>
<FilePath>..\User\lib\user_math.c</FilePath>
</File>
<File>
<FileName>filter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\lib\filter.c</FilePath>
</File>
<File>
<FileName>kalman.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\lib\kalman.c</FilePath>
</File>
</Files>
</Group>
<Group>

11
User/device/djiMotor.c
View File

@ -141,6 +141,17 @@ void djiMotorEncode()
}
break;
}
case GM6020_2:
{
if(motor_chassis[7].msg_cnt<=50)
{
motor_chassis[7].msg_cnt++;
get_motor_offset(&motor_chassis[7], dji_rx_data);
}else{
get_6020_motor_measure(&motor_chassis[7], dji_rx_data);
}
break;
}
default:
{

148
User/module/ball.cpp
View File

@ -7,6 +7,9 @@
extern RC_ctrl_t rc_ctrl;
extern int key;
// C键 sw[4]👆 200 中1000 👇1800
// D键 sw[5]👆 1800 👇200
#define START 0
#define OUT 100
@ -14,9 +17,9 @@ extern int key;
//三摩擦轮运球！！！
//添加平移3508 得跑位置吧
const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0};
const fp32 Ball:: Extend_speed_PID[3] = {5, 0.01, 0};
const fp32 Ball:: Extend_angle_PID[3]= { 5, 0.01, 0};
const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0};
const fp32 Ball:: Extend_speed_PID[3] = { 25, 0, 0.};
const fp32 Ball:: Extend_angle_PID[3]= { 60, 0, 0.1};
int speedm=0;
int speedm1=0;
@ -26,14 +29,23 @@ Ball ::Ball()
{
detect_init();
//伸缩6020
Extern_Motor = get_motor_point(6);
//Extern_Motor->type = GM6020;
PID_init(&extend_speed_pid,PID_POSITION,Extend_speed_PID,3000, 200);
PID_init(&extend_angle_pid,PID_POSITION,Extend_angle_PID,3000, 200);
//两个伸缩6020
Extern_Motor[0] = get_motor_point(6);
Extern_Motor[1] = get_motor_point(7);
Extern_Motor[0]->type = GM6020;
Extern_Motor[1]->type = GM6020;
PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,25000, 2000);
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,25000, 1000);
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,25000, 2000);
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,25000, 1000);
e_result[0] = 0;
e_result[1] = 0;
angleSet[0] = 0;
angleSet[1] = 0;
e_result = 0;
angleSet = 0;
//三摩擦轮
for(int i = 0;i < MOTOR_NUM;i ++)
{
@ -51,6 +63,16 @@ Ball ::Ball()
//状态机状态初始化
currentState1= BALL_IDLE;
// for(int i = 0;i < MOTOR_NUM;i ++)
// {
// //摩擦轮滤波器初始化
// LowPassFilter2p_Init(filter.in + i , 500,
// -1.0f);
// LowPassFilter2p_Init(filter.out + i, 500,
// -1.0f);
// }
}
void Ball :: Filter_init(float target_freq)
@ -67,28 +89,47 @@ void Ball :: Filter_init(float target_freq)
void Ball ::Extend_control(int angle)
{
int16_t delta;
angleSet = angle/2;
int16_t delta[2];
angleSet[0] = angle;
angleSet[1] = -angle;
delta[0] = PID_calc(&extend_angle_pid[0],Extern_Motor[0]->total_angle , angleSet[0]);
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
delta[1] = PID_calc(&extend_angle_pid[1], Extern_Motor[1]->total_angle , angleSet[1]);
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
// delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
// angle_get[0]=motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]);
// angle_get[1]=motor_angle_change(Extern_Motor[1]->real_round, angleSet[1]);
// delta[0] = PID_calc(&extend_angle_pid, angle_get[0], angleSet[0]);
// e_result[0] = PID_calc(&extend_speed_pid, Extern_Motor[0]->speed_rpm, delta[0]);
// delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
fp32 angle_get;
angle_get=motor_angle_change(Extern_Motor->real_round, angleSet);
delta = PID_calc(&extend_angle_pid, angle_get, angleSet);
e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
CAN_cmd_1FF(0,0,e_result,0,&hcan1);
osDelay(1);
}
// void Ball ::Extend_control(int angle)
// {
// int16_t delta;
// angleSet = angle;
void Ball ::Spin(float speed,float speed2)
{
// delta = PID_calc(&extend_angle_pid, Extern_Motor->total_angle, angleSet);
// e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
for(int i = 0;i < MOTOR_NUM;i ++)
// }
void Ball ::Spin(float speed)
{
hand_Motor[i]->speed_rpm=LowPassFilter2p_Apply(filter.in + i, hand_Motor[i]->speed_rpm);
}
speedSet[MOTOR_1] = -speed;
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
@ -96,40 +137,63 @@ void Ball ::Spin(float speed,float speed2)
speedSet[MOTOR_2] = speed;
result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
speedSet[MOTOR_3] = speed2;
speedSet[MOTOR_3] = speed;
result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
for(int i = 0;i < MOTOR_NUM;i ++)
{
result[i]=LowPassFilter2p_Apply(filter.out + i, result[i]);
}
void Ball::ballDown(void)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
speedm=-500;
}
void Ball::ballStop(void)
{
speedm=0;
}
int ball_have = 0;
void Ball::ballTake(void)
{
ball_have=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
speedm=2000;
if(ball_have==1)
{
speedm=0;
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
}
}
int flag =0;
int ball_state = 0;
void Ball::ballHadling(void)
{
ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
switch (currentState1)
{
case BALL_IDLE:
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
if (key > 0) // 检测按键是否被按下
// if (key > 0) // 检测按键是否被按下
if (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
{
speedm=-4500;
speedm1=-4500;
speedm=-4000;
currentState1 = BALL_FORWARD; // 切换到正转状态
}
break;
case BALL_FORWARD:
if ( hand_Motor[MOTOR_1]->speed_rpm >= 4480&&hand_Motor[MOTOR_1]->speed_rpm <= 4530 &&
hand_Motor[MOTOR_2]->speed_rpm <= -4480&&hand_Motor[MOTOR_2]->speed_rpm >= -4530 &&
hand_Motor[MOTOR_3]->speed_rpm <= -4480&&hand_Motor[MOTOR_3]->speed_rpm >= -4530 )
if ( hand_Motor[MOTOR_1]->speed_rpm >= 3980&&hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
hand_Motor[MOTOR_2]->speed_rpm <= -3980&&hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
hand_Motor[MOTOR_3]->speed_rpm <= -3980&&hand_Motor[MOTOR_3]->speed_rpm >= -4020 )
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
currentState1 = BALL_DROP; // 切换到球下落状态
@ -138,11 +202,10 @@ void Ball::ballHadling(void)
case BALL_DROP:
if (ball_state == 1) //读光电 有球 0 无球 1
if (ball_state == 0) //读光电 有球 1 无球 0
{
osDelay(200); // 延时200ms
speedm=4500;
speedm1=4500;
speedm=3500;
currentState1 = BALL_REVERSE; // 切换到反转状态
}
@ -150,11 +213,10 @@ void Ball::ballHadling(void)
case BALL_REVERSE:
if (ball_state == 0)
if (ball_state == 1)
{
speedm=0; // 停止电机
speedm1=0;
currentState1 = BALL_CLOSE; // 切换到完成状态
}
@ -162,7 +224,7 @@ void Ball::ballHadling(void)
case BALL_CLOSE:
// osDelay(200); // 延时50ms
if(ball_state == 0)
if(ball_state == 1)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
currentState1 = BALL_FINISH; // 切换到完成状态
@ -175,7 +237,6 @@ void Ball::ballHadling(void)
key=0; // 重置按键状态
flag=0;
speedm=0;
speedm1=0;
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
currentState1 = BALL_IDLE; // 回到空闲状态
break;
@ -185,11 +246,16 @@ void Ball::ballHadling(void)
break;
}
}
void Ball::Send_control()
{
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
osDelay(1);
CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1);
osDelay(2);
}

19
User/module/ball.hpp
View File

@ -19,6 +19,7 @@ typedef enum {
BALL_FLAG,
BALL_CLOSE, // 关闭状态
BALL_FINISH, // 完成状态
//持球状态
BALL_TAKE,
BALL_LOSE
@ -48,16 +49,19 @@ class Ball
public:
Ball();
void Filter_init(float target_freq);
void Spin(float speed,float speed2);
void Spin(float speed);
void ballHadling(void);
void ballDown(void);
void Send_control(void);
void ballStop(void);
void ballTake(void);
void Extend_control(int angle);
BallState_t currentState1; // 当前状态
//伸缩6020
int16_t e_result;
motor_measure_t * Extern_Motor;
int16_t e_result[2];
motor_measure_t * Extern_Motor[2];
int16_t result[MOTOR_NUM];
motor_measure_t *hand_Motor[MOTOR_NUM];
@ -66,9 +70,10 @@ public:
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
Filter filter;
private:
//滤波一下
Filter filter;
//三个摩擦轮
static const float M3508_speed_PID[3];
@ -82,10 +87,10 @@ private:
//位置环pid
pid_type_def angle_pid[MOTOR_NUM];
pid_type_def extend_speed_pid;
pid_type_def extend_angle_pid;
pid_type_def extend_speed_pid[2];
pid_type_def extend_angle_pid[2];
float angleSet;
float angleSet[2];
float speedSet[MOTOR_NUM];
};

3
User/module/gimbal.cpp
View File

@ -10,7 +10,6 @@
//可活动角度
#define ANGLE_ALLOW 1.0f
extern RC_ctrl_t rc_ctrl;
int angle1 = 0;
NUC_t nuc;
const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1, 0};
@ -32,7 +31,7 @@ Gimbal::Gimbal()
void Gimbal::gimbalFlow()
{
int16_t delta[1];
angleSet = angle1;
//angleSet = angle1;
delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);

104
User/module/shoot.cpp
View File

@ -5,22 +5,33 @@
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "calc_lib.h"
#include "vofa.h"
extern RC_ctrl_t rc_ctrl;
#define SHOOT_SPEED 40000
#define SHOOT_SPEED_BACK -2000
#define STOP 0
#define Trigger 3000
float vofa[8];
#define SHOOT_SPEED 30500
#define SHOOT_SPEED_BACK -2500
#define Error 50
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0.01, 0};
#define STOP 0
#define Trigger_Torque -5000
//sw[7]👆 1694 中 1000 👇306
//sw[2]👆 1694 👇306
//F键 sw[0]👆 1800 中 1000 👇200
//E键 sw[1]👆 1800 👇200
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
int t=0;
Shoot::Shoot()
{
//扳机初始化
trigger_Motor= get_motor_point(4);
trigger_Motor->type=M3508;
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,16000, 10000);//pid初始化
trigger_Motor->type=M2006;
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
speedSet = 0;
result = 0;
@ -31,32 +42,31 @@ Shoot::Shoot()
currentState= SHOOT_IDLE;
}
void Shoot::trigger_spin()
void Shoot::trigger_control()
{
if(t>0)
{
speedSet=Trigger;
if(trigger_Motor->speed_rpm<5)
{
speedSet=-Trigger;
}
}
///speedSet=speed;
//result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
//result = Trigger_Torque;
CAN_cmd_1FF(result,0,0,0,&hcan1);
}
void Shoot::shootThree()
{
if((rc_ctrl.sw[1]>500))
if((rc_ctrl.sw[7]==1694))
{
speed_5065 = SHOOT_SPEED;
}
else
if((rc_ctrl.sw[7]==1000))
{
speed_5065=STOP;
//发一次会堵塞另一个
// CAN_VESC_HEAD(1);
// CAN_VESC_HEAD(2);
}
if(rc_ctrl.sw[5]==1694)
if(rc_ctrl.sw[7]==306)
{
speed_5065=SHOOT_SPEED_BACK;
@ -64,31 +74,33 @@ void Shoot::shootThree()
CAN_VESC_Control(1,speed_5065,&hcan2);
// CAN_VESC_RPM(1, speed_5065);
// CAN_VESC_RPM(2, speed_5065);
// vofa[0] = motor_5065[1]->rotor_speed;
// vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
// vofa_tx_main(vofa); // 发送数据到虚拟串口
}
void Shoot::shootBack()
{
if(rc_ctrl.sw[5]==1694)
{
CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
}
speed_5065=SHOOT_SPEED_BACK;
CAN_VESC_Control(1,speed_5065,&hcan2);
}
void Shoot::shootStop()
{
CAN_VESC_HEAD(1);
CAN_VESC_HEAD(2);
speed_5065=STOP;
CAN_VESC_Control(1,speed_5065,&hcan2);
}
void Shoot::shootStateMachine() {
switch (currentState) {
case SHOOT_IDLE: {
speed_5065=STOP;
result=STOP;
// 空闲状态，等待遥控器输入
if (rc_ctrl.sw[1] > 500) {
if((rc_ctrl.sw[0]==1800)) {
currentState = SHOOT_FIRE; // 切换到发射状态
}
break;
@ -96,21 +108,25 @@ void Shoot::shootStateMachine() {
case SHOOT_FIRE: {
// 发射状态，控制电机发射
speed_5065 = map((float)rc_ctrl.sw[1], 500, 1694, 30000, 45000);
CAN_VESC_RPM(1, speed_5065);
CAN_VESC_RPM(2, speed_5065);
speed_5065 = SHOOT_SPEED;
if(motor_5065[0]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[0]->rotor_speed<=SHOOT_SPEED+Error &&
motor_5065[1]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[1]->rotor_speed<=SHOOT_SPEED+Error)
{
result=Trigger_Torque;//恒力扳机
}
// 检测光电传感器是否触发
if (IS_PHOTOELECTRIC_TRIGGERED()) {
currentState = SHOOT_TRIGGERED; // 切换到光电触发状态
currentState = SHOOT_BACK; // 切换到光电触发状态
}
break;
}
case SHOOT_TRIGGERED: {
case SHOOT_BACK: {
// 光电触发状态，停止发射
CAN_VESC_HEAD(1);
CAN_VESC_HEAD(2);
result=STOP;
speed_5065=STOP;
// 切换到返回状态
currentState = SHOOT_RETURN;
break;
@ -118,11 +134,13 @@ void Shoot::shootStateMachine() {
case SHOOT_RETURN: {
// 自动返回状态，控制电机反转
CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
speed_5065=SHOOT_SPEED_BACK;
result=-Trigger_Torque;
// 检测返回完成（可以通过时间或其他传感器判断）
if (rc_ctrl.sw[1] < 500) { // 假设遥控器开关控制返回完成
if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
speed_5065=SHOOT_SPEED_BACK;
result=STOP;
currentState = SHOOT_IDLE; // 切换回空闲状态
}
break;
@ -134,4 +152,12 @@ void Shoot::shootStateMachine() {
break;
}
}
CAN_VESC_Control(1,speed_5065,&hcan2);
vofa[0] = motor_5065[1]->rotor_speed;
vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
vofa[2] = speed_5065; // 发送电机速度数据
vofa_tx_main(vofa); // 发送数据到虚拟串口
}

10
User/module/shoot.hpp
View File

@ -7,7 +7,7 @@
typedef enum {
SHOOT_IDLE, // 空闲状态
SHOOT_FIRE, // 发射状态
SHOOT_TRIGGERED, // 光电触发状态
SHOOT_BACK, // 光电触发状态
SHOOT_RETURN // 自动返回状态
} ShootState_t;
@ -32,10 +32,14 @@ public:
void shootStop(void);
void shootBack(void);
void shootStateMachine(void);
void trigger_spin(void);
void trigger_control(void);
float speed_5065;
float speed_trigger;
ShootState_t currentState;
int16_t result;
//暂时放在公共,task里使用
CAN_MotorFeedback_t *motor_5065[2];
//==========================公共变量==========================
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
@ -47,10 +51,8 @@ private:
//电机速度pid结构体
pid_type_def speed_pid;
motor_measure_t *trigger_Motor;
int16_t result;
float speedSet;
CAN_MotorFeedback_t *motor_5065[2];
};

73
User/task/ballTask.cpp
View File

@ -8,11 +8,23 @@
#include "vofa.h"
extern RC_ctrl_t rc_ctrl;
Ball ball;
float vofa[8];
//float vofa[8];
int out=0;
// 左旋钮 sw[2] 左1800 右200
// 右旋钮 sw[3] 左1800 右200
// C键 sw[4]👆 200 中1000 👇1800
// D键 sw[5]👆 1800 👇200
int angle1=34;
int angle2=23;
int abc=0;
int a1=0;
int speed_set=0;
int speed_feedback=0;
extern int speedm;
extern int speedm1;
@ -21,7 +33,6 @@ void FunctionBall(void *argument)
(void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
ball.Filter_init(TASK_FREQ_BALL);
uint32_t tick = osKernelGetTickCount();
while(1)
@ -29,20 +40,56 @@ void FunctionBall(void *argument)
#ifdef DEBUG
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
#endif
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
//abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
ball.Extend_control(out);
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
if(rc_ctrl.sw[2]>=1200)
{
angle1=75;
}
if(rc_ctrl.sw[2]<1200)
{
angle1=0;
}
//运球
if(rc_ctrl.sw[3]>=1200)
{
a1=1;
//👇
ball.ballHadling(); // 处理摩擦轮转动
ball.Spin(speedm,speedm1);
}
//下球
if(rc_ctrl.sw[4]==200)
{
//👆
ball.ballDown();
}
//回弹停止
if(rc_ctrl.sw[3]<1000)
{
//👆
ball.ballStop();
}
if(rc_ctrl.sw[7]==1800)
{
ball.ballTake();
}
ball.Extend_control(angle1);
ball.Spin(speedm);
ball.Send_control();
vofa[0] = speedm; // 发送电机角度数据
vofa[1] = -ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
vofa[2] = ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
vofa[3] = ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
vofa_tx_main(vofa); // 发送数据到虚拟串口
// vofa[0] = -speedm; // 发送电机角度数据
// vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
// vofa[2] = -ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
// vofa[3] = -ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
// vofa_tx_main(vofa); // 发送数据到虚拟串口
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick);

2
User/task/gimbalTask.cpp
View File

@ -28,7 +28,7 @@ void FunctionGimbal(void *argument)
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
#endif
cnt1++;
//cnt1++;
// gimbal.gimbalFlow();
// 从消息队列接收视觉数据

32
User/task/shootTask.cpp
View File

@ -9,8 +9,15 @@
extern RC_ctrl_t rc_ctrl;
Shoot shoot;
int shoot_flag = 0;
int a2;
// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2
//F键 sw[0]👆 1800 中 1000 👇200
//E键 sw[1]👆 1800 👇200
void FunctionShoot(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
@ -25,15 +32,36 @@ while(1)
task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());
#endif
//我放的任务通知 运球成功放置过来后
shoot.flag_thread=osThreadFlagsGet();
if(shoot.flag_thread & BALL_OK)
{
shoot.shootThree();
a2=2;
// shoot.shootThree();
}
//shoot.shootBack();
shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
if(rc_ctrl.sw[1]>1000)
{
shoot.shootStateMachine();
if(rc_ctrl.sw[0]==200)
{
shoot.shootBack();
}
if(rc_ctrl.sw[0]==1000)
{
shoot.shootStop();
}
}
if(rc_ctrl.sw[1]==200)
{
shoot.shootStop();
}
shoot.trigger_control();
osDelay(2);
tick += delay_tick; /* 计算下一个唤醒时刻 */