Skip to content

Commit

Permalink
add ZJRoboCon2024TC
Browse files Browse the repository at this point in the history
  • Loading branch information
@Mark-tz
Mark-tz committed May 14, 2024
1 parent 1be508f commit 7279a01
Show file tree
Hide file tree
Showing 17 changed files with 641 additions and 305 deletions.
4 changes: 2 additions & 2 deletions Client/src/vision/ballstate.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,12 @@
#define BALLSTATE_H

enum Result {fail, success, undefined};
enum State {_touched, _shoot, _struggle, _pass, _dribble, _stop, _outoffied, _control, _undefine, _kicked, _chip_pass, _flat_pass};
enum State {_unknown, _touched, _shoot, _struggle, _pass, _dribble, _stop, _outoffied, _control, _undefine, _kicked, _chip_pass, _flat_pass};

class Ballstate {
public:
Ballstate();
State ballState;
State ballState = State::_unknown;
};

#endif // BALLSTATE_H
2 changes: 1 addition & 1 deletion Client/src/vision/collisiondetect.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include "globaldata.h"
#include "geometry.h"

#define DETECT_DIST 10 //detect whether the ball in 300mm of vechile
#define DETECT_DIST 300 //detect whether the ball in 300mm of vechile
#define SPLIT_THRESHOLD 20 //split
#define NEAR_VECHILE_MIN_FRAME 5 //keep frame to detect ball SLIGHTLY touch vechile
#define LENGTH_THERESHOLD 5800
Expand Down
2 changes: 1 addition & 1 deletion Client/src/vision/maintain.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ void CMaintain::run() { //TODO move to visionmodule
// }

//判断碰撞
// CollisionDetect::instance()->analyzeData(result);
CollisionDetect::instance()->analyzeData(result);
//ball kalmanfilter FOLLOW
auto & tempMatrix = ballKalmanFilter.follow(result.ball[0].pos);
DealBall::instance()->updateVel(tempMatrix, result);
Expand Down
302 changes: 151 additions & 151 deletions Core/src/Algorithm/BallStatus.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,151 +1,151 @@
#include "BallStatus.h"
#include <RobotSensor.h>
#include "Global.h"
#include "utils.h"
#include "BallSpeedModel.h"
#include "parammanager.h"
#include "staticparams.h"
namespace{
bool isNearPlayer = false;
bool isEnemyTouch = false;
bool isOurTouch = false;

int standOffCouter=0;
int ourBallCouter=0;


int lastTheirBestPlayer=0;
int lastOurBestPlayer=0;
const int StateMaxNum=5;
int stateCouter[StateMaxNum]={0,0,0,0,0};

bool IS_SIMULATION = false;

const int KICK_OUT_MIN_VELOCITY = 750;
}

CBallStatus::CBallStatus(void):_chipkickstate(false)
{
_ballMovingVel = CVector(0,0);
_isKickedOut = false;
_ballToucher=0;
_ballState=None;
_ballStateCouter=0;

standOffCouter=0;
ourBallCouter=0;
ZSS::ZParamManager::instance()->loadParam(IS_SIMULATION,"Alert/IsSimulation",false);

initializeCmdStored();
}

void CBallStatus::UpdateBallStatus(const CVisionModule* pVision)
{
UpdateBallMoving(pVision);
CheckKickOutBall(pVision);
_contactChecker.refereeJudge(pVision);
_lastBallToucher=_ballToucher;
_ballToucher=_contactChecker.getContactNum();
if (_isKickedOut||_isChipKickOut){
_ballToucher=_kickerNum;
}

}


void CBallStatus::UpdateBallMoving(const CVisionModule* pVision)
{
const ObjectPoseT& ball = pVision->ball(); // 球
isNearPlayer = false;
for (int i=0; i<PARAM::Field::MAX_PLAYER*2; i++){
if (pVision->allPlayer(i).Valid() && pVision->allPlayer(i).Pos().dist(ball.Pos())< PARAM::Field::MAX_PLAYER_SIZE/2+5){
isNearPlayer = true;
break;
}
}
if (false == isNearPlayer && ball.Vel().mod()>20){
// 只有当球不在车附近时,才使用预测后的球速来更新; 以免球在车附近时,将球速设为0;
_ballMovingVel = ball.Vel();
}
else{
// 保留球速，方向使用稳定可靠的方向
double ballspeed = max(1.0, ball.Vel().mod());
_ballMovingVel = Utils::Polar2Vector(ballspeed, _ballMovingVel.dir());
//std::cout<<"BallStatus: Ball Near Player"<<endl;
}
}

// 从PlayInterface中移出的球状态部分
void CBallStatus::initializeCmdStored()
{
for (int i=0; i<PARAM::Field::MAX_PLAYER; i++){
for (int j=0; j<MAX_CMD_STORED; j++){
_kickCmd[i][j].setKickCmd(i,0,0,0);
}
}
}

void CBallStatus::setCommand(CSendCmd kickCmd, int cycle)
{
_kickCmd[kickCmd.num()][cycle % MAX_CMD_STORED] = kickCmd;
}
void CBallStatus::setCommand(int num, int normalKick, int chipKick, unsigned char dribble, int cycle)
{
_kickCmd[num][cycle % MAX_CMD_STORED].setKickCmd(num, normalKick, chipKick, dribble);
}

void CBallStatus::clearKickCmd()
{
for (int i=0; i<PARAM::Field::MAX_PLAYER; i++){
for (int j=0; j<MAX_CMD_STORED; j++){
_kickCmd[i][j].clear();
}
}
}

void CBallStatus::CheckKickOutBall(const CVisionModule* pVision)
{
_isKickedOut = false;
_isChipKickOut = false;
for (int num=0; num < PARAM::Field::MAX_PLAYER; num++){
//利用通讯信息
// bool sensorValid = RobotSensor::Instance()->IsInfoValid(num);
// bool isBallInFoot = RobotSensor::Instance()->IsInfraredOn(num);
int isKickDeviceOn = RobotSensor::Instance()->IsKickerOn(num);
// bool isBallControlled = RobotSensor::Instance()->isBallControled(num);
/*if(sensorValid){
cout<<pVision->Cycle()<<" "<<sensorValid<<" "<<isBallInFoot<<" "<<isKickDeviceOn<<" "<<isBallControlled<<endl;
}*/
// 用于双向通迅出现问题时，且此时红外正常
if (isKickDeviceOn > 0) {
_kickerNum = num;
_isKickedOut = true;
if (isKickDeviceOn == IS_CHIP_KICK_ON) {
_isChipKickOut = true;
}
break;
}
}
if ((pVision->allPlayer(_ballToucher).RawPos() - pVision->rawBall().Pos()).mod()<98) {
_ballChipLine = CGeoLine(pVision->allPlayer(_ballToucher).RawPos(), pVision->allPlayer(_ballToucher).Dir());
}
}


void CBallStatus::clearBallStateCouter(){
memset(stateCouter,0,StateMaxNum*sizeof(int));
_ballState=None;
}

void subStateJudge(int i,int* stateCouter,bool* enterCond,int* keepThreshold,int& _ballState){
if (!enterCond[i]){
stateCouter[i]--;
if (stateCouter[i]==0){
_ballState=None;
memset(stateCouter,0,StateMaxNum*sizeof(int));
}
}else{
stateCouter[i]++;
stateCouter[i]=min(stateCouter[i],keepThreshold[i]);
}
}
// #include "BallStatus.h"
// #include <RobotSensor.h>
// #include "Global.h"
// #include "utils.h"
// #include "BallSpeedModel.h"
// #include "parammanager.h"
// #include "staticparams.h"
// namespace{
// bool isNearPlayer = false;
// bool isEnemyTouch = false;
// bool isOurTouch = false;

// int standOffCouter=0;
// int ourBallCouter=0;


// int lastTheirBestPlayer=0;
// int lastOurBestPlayer=0;
// const int StateMaxNum=5;
// int stateCouter[StateMaxNum]={0,0,0,0,0};

// bool IS_SIMULATION = false;

// const int KICK_OUT_MIN_VELOCITY = 750;
// }

// CBallStatus::CBallStatus(void):_chipkickstate(false)
// {
// _ballMovingVel = CVector(0,0);
// _isKickedOut = false;
// _ballToucher=0;
// _ballState=None;
// _ballStateCouter=0;

// standOffCouter=0;
// ourBallCouter=0;
// ZSS::ZParamManager::instance()->loadParam(IS_SIMULATION,"Alert/IsSimulation",false);

// initializeCmdStored();
// }

// void CBallStatus::UpdateBallStatus(const CVisionModule* pVision)
// {
// UpdateBallMoving(pVision);
// CheckKickOutBall(pVision);
// _contactChecker.refereeJudge(pVision);
// _lastBallToucher=_ballToucher;
// _ballToucher=_contactChecker.getContactNum();
// if (_isKickedOut||_isChipKickOut){
// _ballToucher=_kickerNum;
// }

// }


// void CBallStatus::UpdateBallMoving(const CVisionModule* pVision)
// {
// const ObjectPoseT& ball = pVision->ball(); // 球
// isNearPlayer = false;
// for (int i=0; i<PARAM::Field::MAX_PLAYER*2; i++){
// if (pVision->allPlayer(i).Valid() && pVision->allPlayer(i).Pos().dist(ball.Pos())< PARAM::Field::MAX_PLAYER_SIZE/2+5){
// isNearPlayer = true;
// break;
// }
// }
// if (false == isNearPlayer && ball.Vel().mod()>20){
// // 只有当球不在车附近时,才使用预测后的球速来更新; 以免球在车附近时,将球速设为0;
// _ballMovingVel = ball.Vel();
// }
// else{
// // 保留球速，方向使用稳定可靠的方向
// double ballspeed = max(1.0, ball.Vel().mod());
// _ballMovingVel = Utils::Polar2Vector(ballspeed, _ballMovingVel.dir());
// //std::cout<<"BallStatus: Ball Near Player"<<endl;
// }
// }

// // 从PlayInterface中移出的球状态部分
// void CBallStatus::initializeCmdStored()
// {
// for (int i=0; i<PARAM::Field::MAX_PLAYER; i++){
// for (int j=0; j<MAX_CMD_STORED; j++){
// _kickCmd[i][j].setKickCmd(i,0,0,0);
// }
// }
// }

// void CBallStatus::setCommand(CSendCmd kickCmd, int cycle)
// {
// _kickCmd[kickCmd.num()][cycle % MAX_CMD_STORED] = kickCmd;
// }
// void CBallStatus::setCommand(int num, int normalKick, int chipKick, unsigned char dribble, int cycle)
// {
// _kickCmd[num][cycle % MAX_CMD_STORED].setKickCmd(num, normalKick, chipKick, dribble);
// }

// void CBallStatus::clearKickCmd()
// {
// for (int i=0; i<PARAM::Field::MAX_PLAYER; i++){
// for (int j=0; j<MAX_CMD_STORED; j++){
// _kickCmd[i][j].clear();
// }
// }
// }

// void CBallStatus::CheckKickOutBall(const CVisionModule* pVision)
// {
// _isKickedOut = false;
// _isChipKickOut = false;
// for (int num=0; num < PARAM::Field::MAX_PLAYER; num++){
// //利用通讯信息
// // bool sensorValid = RobotSensor::Instance()->IsInfoValid(num);
// // bool isBallInFoot = RobotSensor::Instance()->IsInfraredOn(num);
// int isKickDeviceOn = RobotSensor::Instance()->IsKickerOn(num);
// // bool isBallControlled = RobotSensor::Instance()->isBallControled(num);
// /*if(sensorValid){
// cout<<pVision->Cycle()<<" "<<sensorValid<<" "<<isBallInFoot<<" "<<isKickDeviceOn<<" "<<isBallControlled<<endl;
// }*/
// // 用于双向通迅出现问题时，且此时红外正常
// if (isKickDeviceOn > 0) {
// _kickerNum = num;
// _isKickedOut = true;
// if (isKickDeviceOn == IS_CHIP_KICK_ON) {
// _isChipKickOut = true;
// }
// break;
// }
// }
// if ((pVision->allPlayer(_ballToucher).RawPos() - pVision->rawBall().Pos()).mod()<98) {
// _ballChipLine = CGeoLine(pVision->allPlayer(_ballToucher).RawPos(), pVision->allPlayer(_ballToucher).Dir());
// }
// }


// void CBallStatus::clearBallStateCouter(){
// memset(stateCouter,0,StateMaxNum*sizeof(int));
// _ballState=None;
// }

// void subStateJudge(int i,int* stateCouter,bool* enterCond,int* keepThreshold,int& _ballState){
// if (!enterCond[i]){
// stateCouter[i]--;
// if (stateCouter[i]==0){
// _ballState=None;
// memset(stateCouter,0,StateMaxNum*sizeof(int));
// }
// }else{
// stateCouter[i]++;
// stateCouter[i]=min(stateCouter[i],keepThreshold[i]);
// }
// }
Loading

0 comments on commit 7279a01

Please sign in to comment.