Skip to content

Commit

Permalink
update for new rem version
Browse files Browse the repository at this point in the history
  • Loading branch information
@JWillegers
JWillegers committed Jun 6, 2024
1 parent 3cfe174 commit 7fdf9f8
Show file tree
Hide file tree
Showing 3 changed files with 15 additions and 18 deletions.
2 changes: 1 addition & 1 deletion Core/Inc/roboteam_embedded_messages
Submodule roboteam_embedded_messages updated 52 files
+1 −1 generator/latest_rem_version.txt
+171 −80 generator/packets.py
+588 −236 include/REM_BaseTypes.h
+35 −34 include/REM_BasestationConfiguration.h
+32 −31 include/REM_BasestationGetConfiguration.h
+32 −31 include/REM_Log.h
+32 −31 include/REM_Packet.h
+35 −34 include/REM_RobotAssuredAck.h
+38 −37 include/REM_RobotAssuredPacket.h
+41 −40 include/REM_RobotBuzzer.h
+233 −138 include/REM_RobotCommand.h
+664 −0 include/REM_RobotCommandTesting.h
+130 −125 include/REM_RobotFeedback.h
+32 −31 include/REM_RobotGetPIDGains.h
+32 −31 include/REM_RobotKillCommand.h
+63 −62 include/REM_RobotMusicCommand.h
+197 −136 include/REM_RobotPIDGains.h
+509 −136 include/REM_RobotSetPIDGains.h
+530 −139 include/REM_RobotStateInfo.h
+19 −19 proto/REM_BasestationConfiguration.proto
+18 −18 proto/REM_BasestationGetConfiguration.proto
+18 −18 proto/REM_Log.proto
+18 −18 proto/REM_Packet.proto
+19 −19 proto/REM_RobotAssuredAck.proto
+20 −20 proto/REM_RobotAssuredPacket.proto
+20 −20 proto/REM_RobotBuzzer.proto
+61 −45 proto/REM_RobotCommand.proto
+109 −0 proto/REM_RobotCommandTesting.proto
+44 −44 proto/REM_RobotFeedback.proto
+18 −18 proto/REM_RobotGetPIDGains.proto
+18 −18 proto/REM_RobotKillCommand.proto
+28 −28 proto/REM_RobotMusicCommand.proto
+56 −48 proto/REM_RobotPIDGains.proto
+104 −48 proto/REM_RobotSetPIDGains.proto
+108 −56 proto/REM_RobotStateInfo.proto
+590 −236 python/REM_BaseTypes.py
+35 −34 python/REM_BasestationConfiguration.py
+32 −31 python/REM_BasestationGetConfiguration.py
+32 −31 python/REM_Log.py
+32 −31 python/REM_Packet.py
+35 −34 python/REM_RobotAssuredAck.py
+38 −37 python/REM_RobotAssuredPacket.py
+41 −40 python/REM_RobotBuzzer.py
+233 −138 python/REM_RobotCommand.py
+665 −0 python/REM_RobotCommandTesting.py
+128 −123 python/REM_RobotFeedback.py
+32 −31 python/REM_RobotGetPIDGains.py
+32 −31 python/REM_RobotKillCommand.py
+63 −62 python/REM_RobotMusicCommand.py
+197 −136 python/REM_RobotPIDGains.py
+509 −136 python/REM_RobotSetPIDGains.py
+530 −139 python/REM_RobotStateInfo.py
2 changes: 1 addition & 1 deletion Core/Src/Util/logging.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
29 changes: 13 additions & 16 deletions Core/Src/robot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
}
Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -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;
Expand Down Expand Up @@ -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];
Expand Down Expand Up @@ -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();
}

Expand Down

0 comments on commit 7fdf9f8

Please sign in to comment.