From 7fdf9f893271d005636855f658e41e7fce2c33ce Mon Sep 17 00:00:00 2001 From: Jonathan <148167.jw@gmail.com> Date: Thu, 6 Jun 2024 10:45:02 +0200 Subject: [PATCH] update for new rem version --- Core/Inc/roboteam_embedded_messages | 2 +- Core/Src/Util/logging.c | 2 +- Core/Src/robot.c | 29 +++++++++++++---------------- 3 files changed, 15 insertions(+), 18 deletions(-) diff --git a/Core/Inc/roboteam_embedded_messages b/Core/Inc/roboteam_embedded_messages index 27e69618..22d83f18 160000 --- a/Core/Inc/roboteam_embedded_messages +++ b/Core/Inc/roboteam_embedded_messages @@ -1 +1 @@ -Subproject commit 27e696188df37742fb24f938496d09348fa77423 +Subproject commit 22d83f18589acb8e8966bf73774004750b392fa4 diff --git a/Core/Src/Util/logging.c b/Core/Src/Util/logging.c index 5ab5ee5f..e24fab39 100644 --- a/Core/Src/Util/logging.c +++ b/Core/Src/Util/logging.c @@ -72,7 +72,7 @@ void LOG(char *message){ MessageContainer* message_container = &message_buffer[index_write]; uint8_t* payload = message_container->payload; - REM_Log_set_header ((REM_LogPayload*) payload, REM_PACKET_TYPE_REM_LOG); + REM_Log_set_packetType ((REM_LogPayload*) payload, REM_PACKET_TYPE_REM_LOG); REM_Log_set_remVersion ((REM_LogPayload*) payload, REM_LOCAL_VERSION); REM_Log_set_payloadSize((REM_LogPayload*) payload, REM_PACKET_SIZE_REM_LOG + message_length); // REM_Log_set_fromBS ((REM_LogPayload*) payload, 1); diff --git a/Core/Src/robot.c b/Core/Src/robot.c index f1ce0702..88e16f73 100644 --- a/Core/Src/robot.c +++ b/Core/Src/robot.c @@ -156,8 +156,8 @@ void Wireless_Readpacket_Cplt(void){ encodeREM_RobotFeedback( (REM_RobotFeedbackPayload*) (txPacket.message + txPacket.payloadLength), &robotFeedback); txPacket.payloadLength += REM_PACKET_SIZE_REM_ROBOT_FEEDBACK; - encodeREM_RobotStateInfo( (REM_RobotStateInfoPayload*) (txPacket.message + txPacket.payloadLength), &robotStateInfo); - txPacket.payloadLength += REM_PACKET_SIZE_REM_ROBOT_STATE_INFO; + // encodeREM_RobotStateInfo( (REM_RobotStateInfoPayload*) (txPacket.message + txPacket.payloadLength), &robotStateInfo); + // txPacket.payloadLength += REM_PACKET_SIZE_REM_ROBOT_STATE_INFO; if(flag_send_PID_gains){ encodeREM_RobotPIDGains( (REM_RobotPIDGainsPayload*) (txPacket.message + txPacket.payloadLength), &robotPIDGains); @@ -188,14 +188,14 @@ void Wireless_RXDone(SX1280_Packet_Status *status){ Wireless_IRQcallbacks SX_IRQcallbacks = { .rxdone = &Wireless_RXDone, .default_callback = &Wireless_Default }; void executeCommands(REM_RobotCommand* robotCommand){ - stateControl_useAbsoluteAngle(robotCommand->useAbsoluteAngle); + stateControl_useAbsoluteAngle(robotCommand->useYaw); float stateReference[4]; stateReference[vel_x] = (robotCommand->rho) * sinf(robotCommand->theta); stateReference[vel_y] = (robotCommand->rho) * cosf(robotCommand->theta); stateReference[vel_w] = robotCommand->angularVelocity; - stateReference[yaw] = robotCommand->angle; + stateReference[yaw] = robotCommand->yaw; stateControl_SetRef(stateReference); - dribbler_SetSpeed(robotCommand->dribbler); + dribbler_SetSpeed(robotCommand->dribblerOn ? 1.0f : 0.0f); shoot_SetPower(robotCommand->kickChipPower); if (robotCommand->doKick) { @@ -208,10 +208,10 @@ void executeCommands(REM_RobotCommand* robotCommand){ shoot_Shoot(shoot_Chip); } } - else if (robotCommand->kickAtAngle) { + else if (robotCommand->kickAtYaw) { float localState[4] = {0.0f}; stateEstimation_GetState(localState); - if (fabs(localState[yaw] - robotCommand->angle) < 0.025) { + if (fabs(localState[yaw] - robotCommand->yaw) < 0.025) { if (ballPosition.canKickBall || robotCommand->doForce) { shoot_Shoot(shoot_Kick); } @@ -224,7 +224,7 @@ void resetRobotCommand(REM_RobotCommand* robotCommand){ } void initPacketHeader(REM_Packet* packet, uint8_t robot_id, uint8_t channel, uint8_t packet_type){ - packet->header = packet_type; + packet->packetType = packet_type; packet->toPC = true; packet->fromColor = channel; packet->remVersion = REM_LOCAL_VERSION; @@ -247,7 +247,7 @@ bool updateTestCommand(REM_RobotCommand* rc, uint32_t time){ // First, empty the entire RobotCommand resetRobotCommand(rc); // Set the basic required stuff - rc->header = REM_PACKET_TYPE_REM_ROBOT_COMMAND; + rc->packetType = REM_PACKET_TYPE_REM_ROBOT_COMMAND; rc->remVersion = REM_LOCAL_VERSION; rc->toRobotId = ROBOT_ID; @@ -266,7 +266,7 @@ bool updateTestCommand(REM_RobotCommand* rc, uint32_t time){ // Rotate around, slowly rc->angularVelocity = 6 * sin(period_fraction * 2 * M_PI); // Turn on dribbler - rc->dribbler = period_fraction; + rc->dribblerOn = true; // Kick a little every block if(0.95 < period_fraction){ rc->doKick = true; @@ -856,8 +856,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { // return; // State estimation - stateInfo.visionAvailable = activeRobotCommand.useCameraAngle; - stateInfo.visionYaw = activeRobotCommand.cameraAngle; // TODO check if this is scaled properly with the new REM messages + stateInfo.visionAvailable = activeRobotCommand.useCameraYaw; + stateInfo.visionYaw = activeRobotCommand.cameraYaw; // TODO check if this is scaled properly with the new REM messages wheels_GetMeasuredSpeeds(stateInfo.wheelSpeeds); stateInfo.xsensAcc[vel_x] = MTi->acc[vel_x]; @@ -910,17 +910,14 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { // robotFeedback.batteryLevel = (batCounter > 1000); robotFeedback.ballSensorWorking = ballSensor_isInitialized(); robotFeedback.ballSensorSeesBall = ballPosition.canKickBall; - robotFeedback.ballPos = ballSensor_isInitialized() ? (-.5 + ballPosition.x / 700.) : 0; float localState[4] = {0.0f}; stateEstimation_GetState(localState); float vu = localState[vel_u]; float vv = localState[vel_v]; robotFeedback.rho = sqrt(vu*vu + vv*vv); - robotFeedback.angle = localState[yaw]; + robotFeedback.yaw = localState[yaw]; robotFeedback.theta = atan2(vu, vv); - robotFeedback.wheelBraking = wheels_GetWheelsBraking(); // TODO Locked feedback has to be changed to brake feedback in PC code - robotFeedback.rssi = last_valid_RSSI; // Should be divided by two to get dBm but RSSI is 8 bits so just send all 8 bits back robotFeedback.dribblerSeesBall = dribbler_GetHasBall(); }