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 */ /*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = up_ball_Pin; GPIO_InitStruct.Pin = up_ball_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT; 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); HAL_GPIO_Init(up_ball_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */ /*Configure GPIO pin : PtPin */
@ -121,7 +121,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin : PtPin */ /*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = STOP_Pin; GPIO_InitStruct.Pin = STOP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(STOP_GPIO_Port, &GPIO_InitStruct); HAL_GPIO_Init(STOP_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */ /*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/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", "nuc.h": "c",
"crc_ccitt.h": "c", "crc_ccitt.h": "c",
"functional": "cpp", "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' Build target 'R1-shooter'
compiling ball.cpp... compiling ball.cpp...
linking... 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... FromELF: creating hex file...
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s). "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> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>angle1,0x0A</ItemText> <ItemText>cnt1,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>k,0x0A</ItemText> <ItemText>cmd_fromnuc</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>7</count> <count>7</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cnt1,0x0A</ItemText> <ItemText>nucbuf</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>8</count> <count>8</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText> <ItemText>nucData</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>9</count> <count>9</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText> <ItemText>send,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>10</count> <count>10</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>nucData</ItemText> <ItemText>\\R1_shooter\../User/task/ballTask.cpp\ball,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>11</count> <count>11</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>send,0x0A</ItemText> <ItemText>abc,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>12</count> <count>12</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>speed1,0x0A</ItemText> <ItemText>ball_state,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>13</count> <count>13</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>\\R1_shooter\../User/task/ballTask.cpp\ball,0x0A</ItemText> <ItemText>flag,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>14</count> <count>14</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>currentState1</ItemText> <ItemText>shoot,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>15</count> <count>15</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>abc,0x0A</ItemText> <ItemText>speedm,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>16</count> <count>16</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>ball_state,0x0A</ItemText> <ItemText>a1,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>17</count> <count>17</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>flag,0x0A</ItemText> <ItemText>a2,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>18</count> <count>18</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>triggerCount,0x0A</ItemText> <ItemText>shoot_flag,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>19</count> <count>19</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>last_ball_state,0x0A</ItemText> <ItemText>angle1,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<WatchWindow2> <WatchWindow2>
@ -957,6 +957,30 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </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>
<Group> <Group>
@ -967,7 +991,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>50</FileNumber> <FileNumber>52</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -979,7 +1003,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>51</FileNumber> <FileNumber>53</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -991,7 +1015,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>52</FileNumber> <FileNumber>54</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1003,7 +1027,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>53</FileNumber> <FileNumber>55</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1015,7 +1039,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>54</FileNumber> <FileNumber>56</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1027,7 +1051,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>55</FileNumber> <FileNumber>57</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1041,13 +1065,13 @@
<Group> <Group>
<GroupName>User/module</GroupName> <GroupName>User/module</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>56</FileNumber> <FileNumber>58</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1059,7 +1083,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>57</FileNumber> <FileNumber>59</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1071,7 +1095,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>58</FileNumber> <FileNumber>60</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1083,7 +1107,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>59</FileNumber> <FileNumber>61</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1103,7 +1127,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>60</FileNumber> <FileNumber>62</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1115,7 +1139,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>61</FileNumber> <FileNumber>63</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1127,7 +1151,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>62</FileNumber> <FileNumber>64</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1139,7 +1163,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>63</FileNumber> <FileNumber>65</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1151,7 +1175,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>64</FileNumber> <FileNumber>66</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1163,7 +1187,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>65</FileNumber> <FileNumber>67</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1175,7 +1199,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>66</FileNumber> <FileNumber>68</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>

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

@ -658,6 +658,16 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\User\lib\user_math.c</FilePath> <FilePath>..\User\lib\user_math.c</FilePath>
</File> </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> </Files>
</Group> </Group>
<Group> <Group>

11
User/device/djiMotor.c
View File

@ -141,6 +141,17 @@ void djiMotorEncode()
} }
break; 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: default:
{ {

152
User/module/ball.cpp
View File

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

21
User/module/ball.hpp
View File

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

3
User/module/gimbal.cpp
View File

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

116
User/module/shoot.cpp
View File

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

12
User/module/shoot.hpp
View File

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

75
User/task/ballTask.cpp
View File

@ -8,11 +8,23 @@
#include "vofa.h" #include "vofa.h"
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
Ball ball; 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 abc=0;
int a1=0;
int speed_set=0;
int speed_feedback=0;
extern int speedm; extern int speedm;
extern int speedm1; extern int speedm1;
@ -21,7 +33,6 @@ void FunctionBall(void *argument)
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
ball.Filter_init(TASK_FREQ_BALL);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
while(1) while(1)
@ -29,20 +40,56 @@ void FunctionBall(void *argument)
#ifdef DEBUG #ifdef DEBUG
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
#endif #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);
ball.ballHadling(); // 处理摩擦轮转动
ball.Spin(speedm,speedm1);
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); // 发送数据到虚拟串口 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(); // 处理摩擦轮转动
}
//下球
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); // 发送数据到虚拟串口
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(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()); task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
#endif #endif
cnt1++; //cnt1++;
// gimbal.gimbalFlow(); // gimbal.gimbalFlow();
// 从消息队列接收视觉数据 // 从消息队列接收视觉数据

32
User/task/shootTask.cpp
View File

@ -9,8 +9,15 @@
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
Shoot shoot; 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 // 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 FunctionShoot(void *argument)
{ {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
@ -25,15 +32,36 @@ while(1)
task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());
#endif #endif
//我放的任务通知 运球成功放置过来后
shoot.flag_thread=osThreadFlagsGet(); shoot.flag_thread=osThreadFlagsGet();
if(shoot.flag_thread & BALL_OK) 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); osDelay(2);
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */