diff --git a/README.md b/README.md index 304a02a..e0529d1 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,17 @@ # r1upper -r1上层 \ No newline at end of file +r1上层 + +## 外设 + +//PE9 刹车光电 + +//PE11 接球气缸 + +//PE13 爪子气缸 + +//PE14 下球气缸 + +//PC6 接球光电 + +//PI6 运球光电 \ No newline at end of file diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index f8c5ed5..f22788d 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -78,9 +78,7 @@ #if DEBUG == 1 //电机回传数据结构体 -motor_measure_t motor_chassis[5]; -motor_measure_t GM6020_motor[2]; -motor_measure_t motor_2006[2]; +motor_measure_t motor_chassis[10]; CAN_MotorFeedback_t motor_5065[2]; #else static motor_measure_t motor_chassis[5]; @@ -299,25 +297,7 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD { return &motor_chassis[i]; } - /** - * @brief 返回GM6020电机数据指针 - * @param[in] i: 电机编号 - * @retval 电机数据指针 - */ - motor_measure_t *get_6020_motor_point(uint8_t i) - { - return &GM6020_motor[i]; - } - /** - * @brief 返回M2006电机数据指针 - * @param[in] i: 电机编号 - * @retval 电机数据指针 - */ - motor_measure_t *get_2006_motor_point(uint8_t i) - { - return &motor_2006[i]; - } - + /** * @brief 限制vesc电机转速 * @param[in/out] rpm: vesce电机转速 diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index 66d010a..9fc5b66 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -133,22 +133,6 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand */ extern motor_measure_t *get_motor_point(uint8_t i); - /** - * @brief 返回GM6020电机数据指针 - * @param[in] i: 电机编号 - * @retval 电机数据指针 - */ - motor_measure_t *get_6020_motor_point(uint8_t i); - - /** - * @brief 返回M2006电机数据指针 - * @param[in] i: 电机编号 - * @retval 电机数据指针 - */ - motor_measure_t *get_2006_motor_point(uint8_t i); - - - /** * @brief 数据处理函数 * @param[in] none diff --git a/User/module/ball.cpp b/User/module/ball.cpp index f3ce1a2..8c2deda 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -4,137 +4,66 @@ #include "detect.h" extern RC_ctrl_t rc_ctrl; -#define KP 0.8 -#define KD 0.008 -//PE9 刹车光电 -//PE11 接球气缸 -//PE13 爪子气缸 -//PE14 下球气缸 -//PC6 接球光电 -//PI6 运球光电 +extern int key; -int aaa=0; // 定义状态机变量 BallState_t currentBallState = BALL_IDLE; +#define MOTOR_SPEED 5000 + +const fp32 Ball:: M3508_speed_PID[3] = {5, 0, 0}; + +//三摩擦轮运球！！！ + Ball ::Ball() -{ - aaa=10; - //ballState =HAL_GPIO_ReadPin(in_ball_GPIO_Port, in_ball_Pin); -detect_init(); - -} - -void Ball ::ballCatch(void) -{ - - if(rc_ctrl.ch[1]>=300) +{ + detect_init(); + for(int i = 0;i < MOTOR_NUM;i ++) + { + hand_Motor[i] = get_motor_point(i); + if(i <=3) { - HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网 - } - else if(rc_ctrl.ch[1]<300) - { - HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网 - } - + hand_Motor[i]->type = M3508;//设置电机类型 + PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 6000);//pid初始化 + } + result[i] = 0; + speedSet[i] = 0; + } + } -void Ball ::ballFlow(void) +void Ball ::Spin(float speed) { - GO_M8010_send_data(&huart6, 1,0,0,angleSet[1],0.5,KP,KD); -} - -void Ball ::ballZero(void) -{ - GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0); -} - -void Ball ::ballHadling(void) -{ - if(rc_ctrl.sw[0]==306) + for(int i = 0;i < MOTOR_NUM;i ++) { - HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);//爪子气缸 开爪 - osDelay(200); - HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);//下球气缸 打出 - osDelay(500); - angleSet[1] = 2; - ballFlow(); + speedSet[i] = speed; + result[i] = PID_calc(&speed_pid[i],hand_Motor[i]->speed_rpm,speedSet[i]); + } - else + CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1); + osDelay(1); + +} + +void Ball::ballHadling(void) +{ + static bool isReversed = false; // 静态变量，记录当前摩擦轮状态，初始为正转 + + if (key > 0) // 检测按键是否被按下 { - HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);//爪子气缸 关爪 - HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);//下球气缸 打入 - angleSet[1] = 0; - ballFlow(); - } + key = 0; // 重置按键状态，防止重复触发 -} - -void Ball::ballStateMachine() { - switch (currentBallState) { - case BALL_IDLE: { - // 空闲状态，等待遥控器输入 - if (rc_ctrl.sw[0] == 306) { // 假设遥控器开关控制开始运球 - // HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 收网 - // osDelay(200); // 等待收网完成 - currentBallState = BALL_RELEASE; // 切换到下球气缸弹出状态 - - } - break; + if (isReversed) + { + // 当前为反转，切换为正转 + Spin(MOTOR_SPEED); // 正转 + isReversed = false; } - - case BALL_RELEASE: { - // 下球气缸弹出，爪子张开 - HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 下球气缸弹出 - HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 爪子张开 - osDelay(200); // 等待动作完成 - currentBallState = BALL_JOINT_REVERSE; // 切换到关节电机反转状态 - break; - } - - case BALL_JOINT_REVERSE: { - // 关节电机反转置水平位置 - angleSet[1] = 2; // 设置关节电机目标角度为水平位置 - ballFlow(); // 控制关节电机动作 - osDelay(500); // 等待电机动作完成 - currentBallState = BALL_FALLING; // 切换到球加速落下状态 - break; - } - - case BALL_FALLING: { - // 球加速落下，等待光电检测 - if (IS_PHOTOELECTRIC_BALL()) { - currentBallState = BALL_WAIT_BOUNCE; // 切换到等待反弹状态 - } - break; - } - - case BALL_WAIT_BOUNCE: { - // 等待光电检测反弹 - osDelay(50); // 简单延时，避免误触发 - if (IS_PHOTOELECTRIC_BALL()) { - currentBallState = BALL_NET_CLOSE; // 切换到收网状态 - } - break; - } - - case BALL_NET_CLOSE: { - // 收网状态 - // HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 收网 - // osDelay(200); // 等待收网完成 - HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 关闭爪子 - HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 下球气缸复位 - angleSet[1] = 1; // 关节电机复位 - ballFlow(); - currentBallState = BALL_IDLE; // 切换回空闲状态 - break; - } - - default: { - - currentBallState = BALL_IDLE; // 默认回到空闲状态 - - break; + else + { + // 当前为正转，切换为反转 + Spin(-MOTOR_SPEED); // 反转 + isReversed = true; } } } diff --git a/User/module/ball.hpp b/User/module/ball.hpp index 013abe6..b44dafa 100644 --- a/User/module/ball.hpp +++ b/User/module/ball.hpp @@ -4,7 +4,8 @@ #include "cmsis_os.h" #include "FreeRTOS.h" #include "bsp_delay.h" -#include "GO_M8010_6_Driver.h" +#include "djiMotor.h" +#include "pid.h" // 定义状态枚举 typedef enum { @@ -16,6 +17,15 @@ typedef enum { BALL_NET_CLOSE // 收网状态 } BallState_t; +typedef enum +{ + MOTOR_1 = 0, + MOTOR_2 = 1, + MOTOR_3 = 2, + MOTOR_DOWN = 3, + MOTOR_NUM +}motorhangd_e; + // 定义光电传感器检测宏 #define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET) @@ -23,23 +33,27 @@ class Ball { public: Ball(); - + void Spin(float speed); void ballHadling(void); - void ballCatch(void); - void ballFlow(void); - void ballStop(void); - void ballZero(void); void ballStateMachine(void); - int ballState; //球状态 0:无球 1:有球 + + int16_t result[MOTOR_NUM]; + private: - //int ballState; //球状态 0:无球 1:有球 + int up_ball_state; //上球状态 0:无球 1:有球 - GO_Motorfield *goData[GO_NUM]; //电机数据指针 - float angleSet[GO_NUM]; //电机目标角度 - float offestAngle[GO_NUM]; //电机初始角度 - float Kp; //比例系数 - float Kd; //微分系数 - float allowRange; //可活动角度 + motor_measure_t *hand_Motor[MOTOR_NUM]; + //三个摩擦轮 + static const float M3508_speed_PID[3]; + static const float M3508_angle_PID[3]; + + //电机速度pid结构体 + pid_type_def speed_pid[MOTOR_NUM]; + //位置环pid + pid_type_def angle_pid[MOTOR_NUM]; + + float angleSet; + float speedSet[MOTOR_NUM]; }; #endif diff --git a/User/module/gimbal.cpp b/User/module/gimbal.cpp index bb7a609..c089503 100644 --- a/User/module/gimbal.cpp +++ b/User/module/gimbal.cpp @@ -10,8 +10,58 @@ //可活动角度 #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}; +const fp32 Gimbal:: Gimbal_angle_PID[3]= { 5, 0.01, 0}; + +#if GM6020ING ==1 +Gimbal::Gimbal() +{ + GM6020_Motor = get_motor_point(6); + GM6020_Motor->type = GM6020; + PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000); + PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000); + + result = 0; + angleSet = 0; + +} + +void Gimbal::gimbalFlow() +{ + int16_t delta[1]; + angleSet = angle1; + delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet); + result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]); + + CAN_cmd_1FF(0,0,result,0,&hcan1); + osDelay(1); + +} + +void Gimbal::gimbalZero() +{ + angleSet=0; + //gimbalFlow(); + +} + +void Gimbal::gimbalVision(const NUC_t &nuc) +{ + int16_t delta[1]; + angleSet = nuc.vision.x; + delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet); + result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]); + + CAN_cmd_1FF(0,0,result,0,&hcan1); + osDelay(1); +} + + + +#else Gimbal::Gimbal() { @@ -60,3 +110,5 @@ void Gimbal::gimbalVision(const NUC_t &nuc) GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD); osDelay(1); } + +#endif diff --git a/User/module/gimbal.hpp b/User/module/gimbal.hpp index 623802a..ce07120 100644 --- a/User/module/gimbal.hpp +++ b/User/module/gimbal.hpp @@ -2,8 +2,11 @@ #define GIMBAL_HPP #include "GO_M8010_6_Driver.h" +#include "djiMotor.h" #include "pid.h" #include "nuc.h" + +#define GM6020ING 1 class Gimbal { public: @@ -11,17 +14,31 @@ public: void gimbalFlow(void);//云台随遥控器转动 void gimbalZero(void);//云台零阻尼模式 void gimbalInit(void);//go初始化 - //void gimbalVision(void);//云台视觉模式 void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据 + + int16_t result; //暂存要发送的扭矩 //float result[GO_NUM]; // float Kp; // float Kd; private: -// //电机速度pid结构体 -// pid_type_def speed_pid[GO_NUM]; -// //位置环pid -// pid_type_def angle_pid[GO_NUM]; + +#if GM6020ING == 1 +//GM6020电机数据 + motor_measure_t *GM6020_Motor; + + static const float Gimbal_speed_PID[3]; + static const float Gimbal_angle_PID[3]; + + //电机速度pid结构体 + pid_type_def speed_pid; + //位置环pid + pid_type_def angle_pid; + + float angleSet; + +#else + motor_measure_t *motorData[GO_NUM]; //视觉发送的要调的角度 float self_angleSet; GO_Motorfield* goData[GO_NUM]; @@ -31,6 +48,8 @@ private: float Kp; float Kd; float allowRange; + +#endif }; diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp index 8942418..3e89adc 100644 --- a/User/task/ballTask.cpp +++ b/User/task/ballTask.cpp @@ -15,10 +15,6 @@ void FunctionBall(void *argument) (void)argument; /* 未使用argument，消除警告 */ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL; - - ball.ballZero(); - ball.ballState=abc; - uint32_t tick = osKernelGetTickCount(); while(1) @@ -26,14 +22,8 @@ void FunctionBall(void *argument) #ifdef DEBUG task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId()); #endif - abc++; - c30=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin); - - - ball.ballHadling(); - ball.ballStateMachine(); - ball.ballCatch(); - + + //ball.ballHadling(); // 处理摩擦轮转动 osDelay(3); tick += delay_tick; /* 计算下一个唤醒时刻 */ diff --git a/User/task/encodeCan.cpp b/User/task/encodeCan.cpp index 68a94f1..261904d 100644 --- a/User/task/encodeCan.cpp +++ b/User/task/encodeCan.cpp @@ -24,7 +24,7 @@ void FunctionCan(void *argument) { #ifdef DEBUG task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId()); - task_struct.freq.can = osKernelGetTickFreq() / (osKernelGetTickCount() - tick); + #endif waitNewDji(); djiMotorEncode(); diff --git a/User/task/gimbalTask.cpp b/User/task/gimbalTask.cpp index 64a7fc4..89165fe 100644 --- a/User/task/gimbalTask.cpp +++ b/User/task/gimbalTask.cpp @@ -10,6 +10,7 @@ Gimbal gimbal; NUC_t nucData; // 用于存储从队列接收的数据 extern RC_ctrl_t rc_ctrl; +int cnt1=0; void FunctionGimbal(void *argument) { @@ -17,9 +18,6 @@ void FunctionGimbal(void *argument) const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL; - HAL_Delay(500); - gimbal.gimbalInit(); - gimbal.gimbalZero(); HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET); uint32_t tick = osKernelGetTickCount(); @@ -28,16 +26,18 @@ void FunctionGimbal(void *argument) { #ifdef DEBUG task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId()); - #endif + #endif + + cnt1++; + + // gimbal.gimbalFlow(); // 从消息队列接收视觉数据 if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK) { // 使用接收到的视觉数据调整云台 gimbal.gimbalVision(nucData); } - - gimbal.gimbalZero(); - gimbal.gimbalFlow(); + osDelay(1); tick += delay_tick; /* 计算下一个唤醒时刻 */ diff --git a/User/task/nucTask.cpp b/User/task/nucTask.cpp index 9d8bf0c..fa36a5b 100644 --- a/User/task/nucTask.cpp +++ b/User/task/nucTask.cpp @@ -30,6 +30,7 @@ void Function_nuc(void *argument) NUC_RawParse(&cmd_fromnuc); +//掉线处理有空写 // if(NUC_WaitDmaCplt()) // { // NUC_RawParse(&cmd_fromnuc); diff --git a/User/task/shootTask.cpp b/User/task/shootTask.cpp index 090ba75..b0a9b7c 100644 --- a/User/task/shootTask.cpp +++ b/User/task/shootTask.cpp @@ -24,16 +24,16 @@ while(1) { #ifdef DEBUG task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId()); -#endif - k++; +#endif + k++; shoot.shootThree(); shoot.shootBack(); - //shoot.shootStateMachine(); - osDelay(2); - tick += delay_tick; /* 计算下一个唤醒时刻 */ - osDelayUntil(tick); + osDelay(2); + + tick += delay_tick; /* 计算下一个唤醒时刻 */ + osDelayUntil(tick); } }