ArduSub custom motor control

[adaniel1]

Hello,

We are a team at Virginia Tech currently developing an autonomous reactor inspection vehicle for Framatome and we intend to use the ArduSub open source solution for our purposes.

In short, we are attempting to use ArduSub’s current implementation to control 4 600HF Hi-Flow Thrusters and then 2 additional PWM pins on the Pixhawk to control 2 additional motors (I can provide specifics on motors if needed.)

Here is what we believe to be the correct approach:

1. We may use the QGroundControl to map a function to a button on our xbox 360 controller. We noticed many of these function calls (i.e. lights1_brighter) are in the joystick.cpp and also found a switch case for custom buttons. We believe that these custom buttons would be what we should use and where we should implement our custom code; one button to tell the the left and right motors to rotate clockwise, and another to do the opposite of that (maybe a third to stop the motors if buttons are not hold-to-operate).

We did also read about some servo_n functions, although we’re not sure if that’s what we need instead of implementing the custom switch cases.

2. By default, 4 PWM pins are assigned to the Pixhawk for 4 thrusters, and we can change this to 6 via the BRD_PWM_COUNT parameter which we believe is part of a parameters text file we upload. We then use the DO_SET_SERVO command to operate the servos (we’re not sure how this function works, what parameters it needs, etc.)

So at this point we’re unsure whether we should be going the custom button route, or the *_servo_n route. We also don’t know if the *_servo_n function that controls the PWM output can send the output specified in (1).

3. Once we know where our code should go (or how to use the *_servo_n function for our purposes), what would this code actually look like? We looked into the motors library to try and get some sense of the calls that functions we found were making to try and use as a reference, but we’re still very unsure of whether it would be correct or not.

One function of interest that seemed to revolve around our intentions was the “AP_MotorsSingle::output_test_seq(motor_seq, pwm)” function in AP_MotorsSingle.cpp, which appears to take in a motor and a pwm value? Although this was also commented to be for testing rather than normal operation.

Alternatively, we also found “AP_Motors::rc_write(chan, pwm)” in AP_motors_class.cpp, prompting for two similar parameters and then following that with what appears to be output_pwm assignment.

2/13/19: Another assignment that seemed interesting is “motor_out[i] = 1500” in AP_Motors6DOF.

Ending Comments

We’re a team of Mechanical Engineers with minimal CS experience. I myself only have some experience with Java, C, C++ from my CS minor which is certainly not enough to fully grasp all of the intricacies of the ArduSub code.

We are hoping that we may find the answers to our many doubts here on the forums, if one is willing to help us out!

Thanks,
Daniel

[jwalser (Jacob)]

(I can provide specifics on motors if needed.)

Hello, what purpose do the two extra motors serve?

[adaniel1]

Each motor is responsible for rotating a drag chain belt spool. Two spools come together to form an arm that descends down to a specific water level. At the end of the arm is a camera provided by Framatome (our sponsor) and its video feed is also handled by the sponsor.

We also have another smaller camera attached to the bottom of the platform, and we plan to setup that feed with QGroundControl.

Link to motors: CIM Motor - VEX Robotics

The image below depicts the drag chain spools, and on the rightmost spool, one of the motors responsible for rotation is featured. Motor functions would most likely be ‘extend’ belt, ‘retract’ belt (if the function is press & hold then a ‘stop’ button would not be required.)

[jwalser (Jacob)]

Ok, you don’t want to use a ‘motor’ output for this, the motors in the context that you have spoken about are specifically for vehicle control.

If this is some actuator you want to be able to move with pilot input, you can just use the ‘servo’ function assignments for the buttons.

Go Hokies.

[adaniel1]

Ok, thanks! That seems like it should be easy enough for us to handle.

At the moment, we’re now thinking that to avoid having to press two buttons simultaneously for the ‘servo’ functions, we could either split the output of one servo pin (if possible) to the two motors so that they receive the same input or implement two of the custom buttons using the servo function implementation as a reference.

[jwalser (Jacob)]

we could either split the output of one servo pin (if possible) to the two motors so that they receive the same input or implement two of the custom buttons using the servo function implementation as a reference.

Yes and yes

[adaniel1]

Thanks!

Are we limited to only discrete steps? Or is there a way to incorporate holding a button down as well? We noticed something along the lines of “if (!held)” but we’re not sure.

Also, are the min/max servo values set to some default somewhere? Or must we ourselves set that value using “chan->set_output_min();” and “chan->set_output_max();” ?

[adaniel1]

We understand the H-bridge is responsible for forward/reverse operations of the motor, but how does one communicate that via the custom function implementation using servo calls?

Hopefully with the answer to these questions, we will be able to implement the function soon.

[williangalvani]

Hi, you can use a brushed ESC, they are basically H-Bridges with a PWM(servo) interface.

[jwalser (Jacob)]

Are we limited to only discrete steps? Or is there a way to incorporate holding a button down as well? We noticed something along the lines of “if (!held)” but we’re not sure.

There are some functions that act each time the button is pushed, and there are some funtions that act as long as the button is pushed down (like servo_max and servo_min).

Also, are the min/max servo values set to some default somewhere? Or must we ourselves set that value using “chan->set_output_min();” and “chan->set_output_max();” ?

These are configured by the SERVOn parameters.

[adaniel1]

Ok based on all this new information, here’s what I think we’ll be doing:

1) Software Installation and Network Configuration (completed)

Installation

2) Wiring
Most of the wiring/connectors have arrived, and we plan on starting this Wednesday.

3) Configure Thrusters
Main 1/ServoChannel1 set to Disabled(33) which is motor 1.
Main 2/ServoChannel2 set to Disabled (34) which is motor 2.
Main 3/ServoChannel3 set to Disabled(35) which is motor 3.
Main 4/ServoChannel4 set to Disabled (36) which is motor 4.

4) Configure Thruster Directions

Initial Setup

5) Configure Custom Motors
BRD_PWM_COUNT set to 2.
Aux 5/ServoChannel9 set to Disabled(0) which is custom motor 1 and corresponds to SERVO_CHAN_1 in the code.
Aux 6/ServoChannel10 set to Disabled (0) which is custom motor 2 and corresponds to SERVO_CHAN_2 in the code.

6) Mode
Switch to Position Hold mode.

7) Button Assignment
Assign k_custom_1, k_custom_2 and k_custom_3 buttons on Xbox 360 controller.

8) Other
Set Voltage and Current Measurement values.
Custom Frame Configuration (if needed).

9) Save Parameters
https://www.ardusub.com/operators-manual/parameters.html

10) Implement Custom Functions

Based on Willian’s reply and some research on how these ESC’s are communicating with servo calls, i’m assuming 800 to 1499 is reverse, 1500 is neutral and 1501 to 2200 is forward?

case JSButton::button_function_t::k_custom_1:
    // this custom function lowers the arm (arm down)

    SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
    ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed

    SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
    ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed

    break;

case JSButton::button_function_t::k_custom_2:
    // this custom function raises the arm (arm up)

    SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
    ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed

    SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
    ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed

    break;

11) Upload The Code
https://www.ardusub.com/developers/developers.html

Questions
a) I think the order would be from top to bottom of this list, or is there something that is out of order?

b) What step should calibration be?

**?) Calibration**
Any calibrations needed for sensors and controller.

c) Not so much of an issue now, but will Position Hold mode work without GPS? We just need the vehicle to hold its position until further input. Would an out-of-water (underwater not within our budget) GPS be required to be able to use Position Hold mode? And if so, what should we use and how to set it up?

d) Is the custom code in the right direction? I just copied the min/max calls for each servo, and so while k_custom_1 or k_custom_2 buttons are held down, the motors should run, and then stop after releasing the button?

I think another solution would be to just call handle_jsbutton_press(k_servo_n_max, false, true)
handle_jsbutton_press(k_servo_n_min, false, true)

[jwalser (Jacob)]

@adaniel1

3) Configure Thrusters
Main 1/ServoChannel1 set to Disabled(33) which is motor 1.
Main 2/ServoChannel2 set to Disabled (34) which is motor 2.
Main 3/ServoChannel3 set to Disabled(35) which is motor 3.
Main 4/ServoChannel4 set to Disabled (36) which is motor 4.

Don’t do this.

@adaniel1

5) Configure Custom Motors
BRD_PWM_COUNT set to 2.
Aux 5/ServoChannel9 set to Disabled(0) which is custom motor 1 and corresponds to SERVO_CHAN_1 in the code.
Aux 6/ServoChannel10 set to Disabled (0) which is custom motor 2 and corresponds to SERVO_CHAN_2 in the code.

Don’t do this.

@adaniel1

6) Mode
Switch to Position Hold mode.

Don’t do this.

@adaniel1

8) Other
Set Voltage and Current Measurement values.
Custom Frame Configuration (if needed).

This should be done before configuring thruster directions

@adaniel1

9) Save Parameters
Redirecting...

Not necessary.

@adaniel1

10) Implement Custom Functions

I recommend you start with a standard build until you learn how this works.

@adaniel1

b) What step should calibration be?

Calibration is not explicitly necessary, don’t worry about it for now.

@adaniel1

c) Not so much of an issue now, but will Position Hold mode work without GPS? We just need the vehicle to hold its position until further input. Would an out-of-water (underwater not within our budget) GPS be required to be able to use Position Hold mode? And if so, what should we use and how to set it up?

No, it’s not stable.

@adaniel1

d) Is the custom code in the right direction? I just copied the min/max calls for each servo, and so while k_custom_1 or k_custom_2 buttons are held down, the motors should run, and then stop after releasing the button?

You don’t have any code to make the motors stop at any point.

[adaniel1]

Thanks for the reply! For the following steps,

[3] Is it that it’s already configured and we just need to connect the thrusters to main 1,2,3 and 4? Or is that the wrong connection to make?

[5] Basically same question as 3, is it simply not needed or are we doing something incorrectly?

[6 and c] So will the vehicle be able to hold it’s position at all? We were hoping for some autonomy in our solution.

[8] Understood! We’ll make sure to do that.

[9] Ok! Are they auto-saved?

[10] That is our plan actually, to get the 4 thrusters working and the 2 motors will follow that, so we still have time to attempt to fully grasp this.

[d] Maybe I misunderstood your earlier reply about how the buttons work. So we’ll need a third button after-all, I am thinking just assign a third custom button like so:

case JSButton::button_function_t::k_custom_3:
// this custom function stops the arm (stop)

    SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
    ServoRelayEvents.do_set_servo(SERVO_CHAN_1, 1500); // 1-indexed

    SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
    ServoRelayEvents.do_set_servo(SERVO_CHAN_2, 1500); // 1-indexed

    break;

[jwalser (Jacob)]

@adaniel1

[3] Is it that it’s already configured and we just need to connect the thrusters to main 1,2,3 and 4? Or is that the wrong connection to make?

Yes, it’s already configured, just connect the motors to the outputs.

@adaniel1

[5] Basically same question as 3, is it simply not needed or are we doing something incorrectly?

It’s not needed. Connect your servos to aux 1 and 2. These correspond to the servo1* and servo2* joystick functions.

@adaniel1

[6 and c] So will the vehicle be able to hold it’s position at all? We were hoping for some autonomy in our solution.

Position enabled modes are not stable. You will not be able to do anything without a current gps position.

@adaniel1

[9] Ok! Are they auto-saved?

Yes

@adaniel1

[10] That is our plan actually, to get the 4 thrusters working and the 2 motors will follow that, so we still have time to attempt to fully grasp this.

You don’t need a third button if you use the ‘while button is held down’ logic.

[adaniel1]

Understood!

Well, I think at this point it may be too late to alter our design plans and so we’ll have to present the product with the missing feature. Is the statement below accurate?

“Position Hold out-of-water is unstable and currently under development by ArduSub, while position hold using the Underwater GPS Developer Kit would be possible but not within the project budget.”

Got the new switch cases written up with the while loop. This would make multiple calls for the same thing, is this acceptable? How/where is the boolean being updated to reflect the current status of the button? (I placed the stop motors code right outside the while loop assuming that somehow it’s updated within the loop).

Also, is it possible to limit the speed of the thrusters? I know for the servos we can specific values between 800 and 2200 but what about for the actual motors?

    case JSButton::button_function_t::k_custom_1:
    // this custom function lowers the arm (arm down)

    while (held) {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed

        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed
    }

     SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
     ServoRelayEvents.do_set_servo(SERVO_CHAN_1, 1500); // 1-indexed

     SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
     ServoRelayEvents.do_set_servo(SERVO_CHAN_2, 1500); // 1-indexed

    break;

    case JSButton::button_function_t::k_custom_2:
    // this custom function raises the arm (arm up)

    while (held) {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed

        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed
    }

     SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
     ServoRelayEvents.do_set_servo(SERVO_CHAN_1, 1500); // 1-indexed

     SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
     ServoRelayEvents.do_set_servo(SERVO_CHAN_2, 1500); // 1-indexed

    break;

[adaniel1]

I am currently also attempting to setup the build environment (get a head start while we work on wiring and finishing up the code). I installed both Cygwin and the GCC compiler. I was able to successfully install empy and pyserial, but the final command does not appear to be running successfully.

pip2 install pymavlink

[jwalser (Jacob)]

There is not enough information about the problem here. Try posting the complete output like demonstrated here:

Waf ArduSub build not working - which board?

Please provide the entire output to these commands in pastebin: git clone https://github.com/ardupilot/ardupilot \ && cd ardupilot \ && git fetch --tags \ && git checkout ArduSub-stable \ && git submodule update --init --recursive \ && ./waf configure --board=px4-v2 \ && ./waf sub

I recommend using anything other than Windows for software development.

[adaniel1]

Here’s the full output:

$ pip2 install pymavlinkDEPRECATION: Python 2.7 will reach the end of its life...

Most of us have windows, if I have enough space I could get virtual box and a linux OS, is windows a serious issue?

Also, any comments on ArduSub custom motor control - #15 by adaniel1

[jwalser (Jacob)]

Software development on windows is difficult to get tooolchain and dependencies set up. Linux is free, and is much easier to get things working. Case in point, you’ve got some installation problem here due to a missing dependency. I don’t know how to fix it. What instructions are you following? You should be doing this: Setting up the Build Environment on Windows 10/11 using WSL1 or WSL2 — Dev documentation

@adaniel1

This would make multiple calls for the same thing, is this acceptable? How/where is the boolean being updated to reflect the current status of the button?

Looks fine. It’s updated asynchronously as QGC streams joystick data.

@adaniel1

Also, is it possible to limit the speed of the thrusters? I know for the servos we can specific values between 800 and 2200 but what about for the actual motors?

A servo signal and motor signal are the same. You may do exactly the same thing with a motor.

[adaniel1]

Thanks!

This is the guide I was following:

http://ardupilot.org/dev/docs/building-setup-windows.html#building-setup-windows

But I i’ll try the link you provided, and if that fails, i’ll install Ubuntu and try from there.