Turning BlueROV into Spiderman

Blue Robotics Software
, , ,
9

Hello everyone,

Today, I have finally found a solution to this: I have overwritten Acrobatics Mode with my own code (because we weren’t using it anyways and it seemed closest to what we were trying to achieve). This is now my version of Sub::acro_run() which is located in control_acro.cpp.

// acro_run - runs the acro controller
// should be called at 100hz or more
void Sub::acro_run()
{
    // if not armed set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        return;
    }

    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    if (pitch_and_dock) {
        //we're still trying to maneuver ourselves into the correct position
        //pitch to 90 degrees and let the ROV move around
        attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 8900.0F, 0.0F, true);
        motors.set_throttle(channel_throttle->norm_input());
        motors.set_forward(channel_forward->norm_input());
        motors.set_lateral(channel_lateral->norm_input());
    } else {
        //we are ready to dock to the wall
        motors.set_throttle(0.5 - (gain/2));
    }

    //set speed of direction via these functions
    //motors.set_throttle(0.5F); //neutral throttle
    //motors.set_forward(0.0F);  //positive is forward, negative is backward
    //motors.set_lateral(0.0F);  //positive is right, negative is left

    //setting roll/pitch/yaw via motors functions does NOT work
    //they seem to ignore the values and instead send the ROV shooting downwards
    //motors.set_roll(0.0F);
    //motors.set_pitch(0.0F);
    //motors.set_yaw(0.0F);

    //attempt to set pitch, avoid gimbal lock
    //attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 89.0F, 0.0F, true);
    //apparently it wants degrees * 100 for some reason
    //attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 8900.0F, 0.0F, true);
}

I have also added a bool pitch_and_dock to Sub.h that gets toggled in case JSButton::button_function_t::k_custom_1 of joystick.cpp.

If anybody wants to play around with this in the future, here is a list of things that didn’t work:

  • implementing a custom function in joystick.cpp - because it is not called frequently enough to keep the thrusters spinning continuously.
  • implementing UserCode.cpp - because it seems to compete with whatever mode is running to control the ROV.
  • using pymavlink - because it does not want to run at the same time as QGC, even if I change the UDP port.
  • using any other mode - see my answer above.

Hope this helps :slight_smile:

4 Likes