Skip to content

Commit

Permalink
Merge pull request #95 from RoboTeamTwente/feature/drain-battery
Browse files Browse the repository at this point in the history 
Allow to safely drain batteries with the robots
  • Loading branch information
@supertom01
supertom01 committed Aug 23, 2023
2 parents 2198520 + f01ab1b commit 1830ed2
Show file tree
Hide file tree
Showing 4 changed files with 36 additions and 8 deletions.
2 changes: 1 addition & 1 deletion Core/Inc/peripherals/buzzer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ void buzzer_Play_Startup();
void buzzer_Play_Tetris();
void buzzer_Play_Mario();
void buzzer_Play_PinkPanther();
void buzzer_Play_PowerUp();
void buzzer_Play_BatteryDrainWarning();
void buzzer_Play_WarningOne();
void buzzer_Play_WarningTwo();
void buzzer_Play_WarningThree();
Expand Down
2 changes: 1 addition & 1 deletion Core/Inc/peripherals/buzzer_tunes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ extern song_struct quickBeepDown[];
extern song_struct startup_song[];
extern song_struct tetris[];
extern song_struct mario_victory[];
extern song_struct powerUp[];
extern song_struct batteryDrainWarning[];
extern song_struct warningOne[];
extern song_struct warningTwo[];
extern song_struct warningThree[];
Expand Down
6 changes: 3 additions & 3 deletions Core/Src/peripherals/buzzer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,8 @@ void buzzer_Play_PinkPanther(){
buzzer_Play(song);
}

void buzzer_Play_PowerUp(){
song = powerUp;
void buzzer_Play_BatteryDrainWarning(){
song = batteryDrainWarning;
buzzer_Play(song);
}

Expand Down Expand Up @@ -193,7 +193,7 @@ song_struct quickBeepDown[] = {{buzz_C5, 0.1F}, {buzz_C4, 0.1F}, {0xFFFF, 0}};
song_struct startup_song[] = {{buzz_C4, 0.1F}, {buzz_D4, 0.1F}, {buzz_E4, 0.1F}, {buzz_F4, 0.1F}, {buzz_G4, 0.1F}, {buzz_A4, 0.1F}, {buzz_B4, 0.1F}, {buzz_C5, 0.1F}, {0xFFFF, 0}};
song_struct tetris[] = {{buzz_E5, Tbeat}, {buzz_B4, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_D5, Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_B4, 0.5F*Tbeat}, {buzz_A4, Tbeat}, {buzz_A4, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_E5, Tbeat}, {buzz_D5, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_B4, Tbeat}, {buzz_Si, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_D5, Tbeat}, {buzz_E5, Tbeat}, {buzz_C5, Tbeat}, {buzz_A4, Tbeat}, {buzz_A4, Tbeat}, {buzz_Si, 1.5F*Tbeat}, {buzz_D5, Tbeat}, {buzz_F5, 0.5F*Tbeat}, {buzz_A5, 0.5F*Tbeat}, {buzz_A5, 0.5F*Tbeat}, {buzz_G5, 0.5F*Tbeat}, {buzz_F5, 0.5F*Tbeat}, {buzz_E5, Tbeat}, {buzz_Si, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_E5, Tbeat}, {buzz_D5, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_B4, Tbeat}, {buzz_B4, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_D5,Tbeat}, {buzz_E5,Tbeat}, {buzz_C5,Tbeat}, {buzz_A4,Tbeat}, {buzz_A4,Tbeat},{buzz_E5,Tbeat}, {buzz_B4, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_D5,Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_B4, 0.5F*Tbeat}, {buzz_A4,Tbeat}, {buzz_A4, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_E5,Tbeat}, {buzz_D5, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_B4, Tbeat}, {buzz_Si, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_D5,Tbeat}, {buzz_E5,Tbeat}, {buzz_C5,Tbeat}, {buzz_A4,Tbeat}, {buzz_A4,Tbeat}, {buzz_Si,1.5F*Tbeat}, {buzz_D5, Tbeat}, {buzz_F5, 0.5F*Tbeat}, {buzz_A5, 0.5F*Tbeat}, {buzz_A5, 0.5F*Tbeat}, {buzz_G5, 0.5F*Tbeat}, {buzz_F5, 0.5F*Tbeat}, {buzz_E5, Tbeat}, {buzz_Si, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_E5,Tbeat}, {buzz_D5, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_B4, Tbeat}, {buzz_B4, 0.5F*Tbeat}, {buzz_C5, 0.5F*Tbeat}, {buzz_D5,Tbeat}, {buzz_E5,Tbeat}, {buzz_C5,Tbeat}, {buzz_A4,Tbeat}, {buzz_A4,Tbeat}, {buzz_E4,2*Tbeat}, {buzz_C4,2*Tbeat}, {buzz_D4,2*Tbeat}, {buzz_B3,2*Tbeat}, {buzz_C4,2*Tbeat}, {buzz_A3,2*Tbeat}, {buzz_GS3,2*Tbeat}, {buzz_B3,Tbeat}, {buzz_Si,Tbeat}, {buzz_E4,2*Tbeat}, {buzz_C4,2*Tbeat}, {buzz_D4,2*Tbeat}, {buzz_B3,2*Tbeat}, {buzz_C4,Tbeat}, {buzz_E4,Tbeat}, {buzz_A4,2*Tbeat}, {buzz_GS4,3*Tbeat}, {buzz_Si,Tbeat}, {0xFFFF, 0}};
song_struct mario_victory[] = {{buzz_GS3,Mbeat/3.0F}, {buzz_CS4,Mbeat/3.0F}, {buzz_F4,Mbeat/3.0F}, {buzz_GS4,Mbeat/3.0F}, {buzz_CS5,Mbeat/3.0F}, {buzz_F5,Mbeat/3.0F}, {buzz_GS5,Mbeat}, {buzz_F5,Mbeat}, {buzz_A3,Mbeat/3.0F}, {buzz_CS4,Mbeat/3.0F}, {buzz_E4,Mbeat/3.0F}, {buzz_A4,Mbeat/3.0F}, {buzz_CS5,Mbeat/3.0F}, {buzz_E5,Mbeat/3.0F}, {buzz_A5,Mbeat}, {buzz_E5,Mbeat}, {buzz_B3,Mbeat/3.0F}, {buzz_DS4,Mbeat/3.0F}, {buzz_FS4,Mbeat/3.0F}, {buzz_B4,Mbeat/3.0F}, {buzz_DS5,Mbeat/3.0F}, {buzz_FS5,Mbeat/3.0F}, {buzz_B5,Mbeat}, {buzz_B5,Mbeat/3.0F}, {buzz_B5,Mbeat/3.0F}, {buzz_B5,Mbeat/3.0F}, {buzz_CS6,Mbeat*2.0F},{0xFFFF, 0}};
song_struct powerUp[] = {{buzz_C5,fBeat*0.5F},{buzz_G4,fBeat},{buzz_C5,fBeat},{buzz_E5,fBeat},{buzz_G5,fBeat},{buzz_C6,fBeat},{buzz_G5,fBeat},{buzz_C5,fBeat*0.5F},{buzz_GS4,fBeat},{buzz_C5,fBeat},{buzz_DS5,fBeat},{buzz_GS5,fBeat},{buzz_DS5,fBeat},{buzz_GS5,fBeat},{buzz_C6,fBeat},{buzz_DS6,fBeat},{buzz_GS6,fBeat},{buzz_DS6,fBeat},{buzz_D5,0.5F*fBeat},{buzz_AS4,fBeat},{buzz_D5,fBeat},{buzz_F5,fBeat},{buzz_AS5,fBeat},{buzz_F5,fBeat},{buzz_AS5,fBeat},{buzz_D6,fBeat},{buzz_F6,fBeat},{buzz_AS6,fBeat},{buzz_F6,fBeat},{0xFFFF, 0}};
song_struct batteryDrainWarning[] = {{buzz_C5,fBeat*0.5F},{buzz_G4,fBeat},{buzz_C5,fBeat},{buzz_E5,fBeat},{buzz_G5,fBeat},{buzz_C6,fBeat},{buzz_G5,fBeat},{buzz_C5,fBeat*0.5F},{buzz_GS4,fBeat},{buzz_C5,fBeat},{buzz_DS5,fBeat},{buzz_GS5,fBeat},{buzz_DS5,fBeat},{buzz_GS5,fBeat},{buzz_C6,fBeat},{buzz_DS6,fBeat},{buzz_GS6,fBeat},{buzz_DS6,fBeat},{buzz_D5,0.5F*fBeat},{buzz_AS4,fBeat},{buzz_D5,fBeat},{buzz_F5,fBeat},{buzz_AS5,fBeat},{buzz_F5,fBeat},{buzz_AS5,fBeat},{buzz_D6,fBeat},{buzz_F6,fBeat},{buzz_AS6,fBeat},{buzz_F6,fBeat},{0xFFFF, 0}};
song_struct warningOne[] = {{buzz_A5,0.1F},{buzz_E5,0.2F},{buzz_Si,0.1F},{buzz_A5,0.1F},{buzz_E5,0.2F},{buzz_Si,0.1F},{buzz_A5,0.1F},{buzz_E5,0.2F},{buzz_Si,0.1F},{0xFFFF, 0}};
song_struct warningTwo[] = {{buzz_C5,0.1F},{buzz_FS5,0.1F},{buzz_B5,0.6F},{buzz_C5,0.1F},{buzz_FS5,0.1F},{buzz_B5,0.1F},{buzz_Si,0.5F},{buzz_C5,0.1F},{buzz_FS5,0.1F},{buzz_B5,0.1F},{buzz_Si,0.5F},{buzz_C5,0.1F},{buzz_FS5,0.1F},{buzz_B5,0.1F},{buzz_Si,0.5F},{0xFFFF, 0}};
song_struct warningThree[] = {{buzz_FS6,0.66F},{buzz_D6,0.66F},{buzz_B5,0.66F},{buzz_G5,0.66F},{buzz_AS5,1.33F},{buzz_Si,1.76F},{buzz_FS6,0.66F},{buzz_D6,0.66F},{buzz_B5,0.66F},{buzz_G5,0.66F},{buzz_AS5,1.33F},{buzz_Si,1.76F},{buzz_FS6,0.66F},{buzz_D6,0.66F},{buzz_B5,0.66F},{buzz_G5,0.66F},{buzz_AS5,1.33F},{buzz_Si,1.76F},{0xFFFF, 0}};
Expand Down
34 changes: 31 additions & 3 deletions Core/Src/robot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ volatile bool ENABLE_UART_PC = true;

volatile bool IS_RUNNING_TEST = false;
volatile bool ROBOT_INITIALIZED = false;
volatile bool DRAIN_BATTERY = false;

MTi_data* MTi;

Expand Down Expand Up @@ -311,6 +312,7 @@ void init(void){
ROBOT_CHANNEL = read_Pin(FT1_pin) == GPIO_PIN_SET ? BLUE_CHANNEL : YELLOW_CHANNEL;
IS_RUNNING_TEST = read_Pin(FT0_pin);
ENABLE_UART_PC = read_Pin(FT2_pin);
DRAIN_BATTERY = read_Pin(FT3_pin);


initPacketHeader((REM_Packet*) &activeRobotCommand, ROBOT_ID, ROBOT_CHANNEL, REM_PACKET_TYPE_REM_ROBOT_COMMAND);
Expand Down Expand Up @@ -435,12 +437,20 @@ void init(void){
HAL_Delay(100);
}

// Check if we are draining the battery. If so, sound an alarm
if(DRAIN_BATTERY) {
LOG("[init:"STRINGIZE(__LINE__)"] In drain mode! Flip pin FT3 and reboot to disable.");
LOG_sendAll();
buzzer_Play_PowerUp();
HAL_Delay(1000);
}

set_Pin(LED5_pin, 1);

// Tell the SX to start listening for packets. This is non-blocking. It simply sets the SX into receiver mode.
// SX1280 section 10.7 Transceiver Circuit Modes Graphical Illustration
// Ignore packets when we're in test-mode by simply never entering this receive-respond loop
if(!IS_RUNNING_TEST) WaitForPacket(SX);
// Ignore packets when we're in test- or battery drain mode by simply never entering this receive-respond loop
if(!(IS_RUNNING_TEST || DRAIN_BATTERY)) WaitForPacket(SX);

// Ensure that the speaker is stopped. The speaker keeps going even if the robot is reset
speaker_Stop();
Expand Down Expand Up @@ -504,7 +514,7 @@ void loop(void){
/* === Determine HALT state === */
xsens_CalibrationDone = (MTi->statusword & (0x18)) == 0; // if bits 3 and 4 of status word are zero, calibration is done
halt = !xsens_CalibrationDone || !(is_connected_wireless || is_connected_serial) || !REM_last_packet_had_correct_version;
if(IS_RUNNING_TEST) halt = false;
if(IS_RUNNING_TEST || DRAIN_BATTERY) halt = false;

if (halt) {
// LOG_printf("HALT %d %d %d\n", xsens_CalibrationDone, checkWirelessConnection(), isSerialConnected);
Expand Down Expand Up @@ -842,6 +852,24 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
stateControl_Update();

wheels_SetSpeeds( stateControl_GetWheelRef() );

// In order to drain the battery as fast as possible we instruct the wheels to go their maximum possible speeds.
// However, for the sake of safety we make sure that if the robot actually turns it immediately stops doing this, since you
// only want to execute this on a roll of tape.
//
// TODO: Once the battery meter has been implemented in software, it would perhaps be nice to stop the drainaige at programmable level.
// Currently you are stuck on the automated shutdown value that is controlled by the powerboard.
if(DRAIN_BATTERY){

// Instruct each wheel to go 30 rad/s
float wheel_speeds[4] = {30.0f * M_PI, 30.0f * M_PI, 30.0f * M_PI, 30.0f * M_PI};
wheels_SetSpeeds(wheel_speeds);

// If the gyroscope detects some rotational movement, we stop the drainage program.
if (fabs(MTi->gyr[2]) > 0.3f) {
DRAIN_BATTERY = false;
}
}
wheels_Update();

/* == Fill robotFeedback packet == */ {
Expand Down

0 comments on commit 1830ed2

Please sign in to comment.