ArduSub custom motor control

Blue Robotics Software ArduSub
42

We tested the servo today…

ESC: https://www.amazon.com/gp/product/B01LZHBJ85/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&psc=1|

Motor: CIM Motor - VEX Robotics

Results:

  • The custom code causes a loss of communication to the pixhawk (QGroundControl announces “Communication Lost”), not sure what the problem is since you approved it?
  • Even when attemptings the servo1_max and servo1_min values, the motor was not really performing as expected.
  • We assumed this was because maybe the pwm values are incorrect? So we tried to send some pwm values using the camera menu and found that certain min/max pwm values cause the motor to configure itself differently. For example, at one stage, a pwm value of 1500 was neutral and then 1300 would cause it to spin in one direction and 1600 in another direction. But as soon as we assigned say 1800 or something, the motor would beep and suddenly 800 is neutral, 1200 causes to spin one direction and then beyond say 1500 it would reconfigure again. So we are very confused about this and think we definitely need someone more experienced to troubleshoot with us.
43

Should I create a new post as ArduSub custom motor control - #42 by adaniel1 is a completely different issue?

44

In order to help you at this point, we need

  1. your current code (git diff)
  2. your esc documentation

There is probably something wrong with the esc calibration. You could use the motors tab + a motor output on the pixhawk to calibrate the esc.

Recommended equipment to help you diagnose/set things up:

  • servo tester
  • esc programmer (if available for your escs)
  • logic analyzer

Should I create a new post as ArduSub custom motor control is a completely different issue?

I don’t think so. It’s the @adaniel1 thread.

1 Like
45

ESC Docs and Specs: http://www.hobbywing.com/goods.php?id=358

Shopping Cart (although I think I will also ask around campus to see if I can find anything):

Servo Tester: Amazon.com

Logic Analyser: Amazon.com

ESC Programmer: hobbywing LED Program Card For Cars & Trucks, xerun, ezrun, quicrun esc - HOBBYWING North America

For the git diff, which two files should I be comparing? I commited my most recent changes so git diff isn’t really showing anything when I call it:

joystick.cpp

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); // this is just to initialize
int onlyOnce = 0;

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

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

                    onlyOnce++;
            }
    }

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

    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)
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // this is just to initialize
int onlyOnce = 0;

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

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

                    onlyOnce++;
            }
    }

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

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

    break;
}

AP_Motors6DOF.cpp

case SUB_FRAME_CUSTOM:
// Put your custom motor setup here
add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, -1.0f, 0, 0, -1.0f, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, -1.0f, 0, -1.0f, 0, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, -1.0f, 0, 1.0f, 0, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, -1.0f, 0, 0, 1.0f, 4);
break;

Motor configuration image:

image

46

Hi,

To use git diff, you need to use as argument your start point commit, that’s probably Sub-3.5 or master. You can use tig to see which one you did use.
And, to make sure that you are not working over the same point, use git diff origin/Sub-3.5 to use the remote version.

47

Ok, I installed tig, and found that origin/Sub-3.5 is indeed the start point.

git diff results: [dan@localhost ardupilot]$ git diff origin/Sub-3.5diff --git a/ArduSub/joystic - Pastebin.com

48

You have a pretty major bug in your code right now: while(held) This code is blocking. It will stop everything running on the autopilot and enter an infinite loop. This is why you loose connection with QGroundControl. All communication from the autopilot stops as it sits in this while(held) loop. Please make your while button is held logic match what is done in the rest of the file for this situation.

49

Didn’t we conclude that the ‘held’ boolean was updated sometime while in the loop once the user releases the button? I don’t see why it would result in an infinite loop if that were the case.

Other uses of the ‘held’ boolean use if statements rather than while loops, so unless I understand exactly how to exit the while loop, I don’t have much to go by for creating a while loop just from looking at the other code.

neighbouring code are using the if statement logic, and going by what I am seeing for other switch statements, I can’t really make sense of how this works with the controller. For k_servo_1_min_toggle for example, when does ‘held’ update to the actions on the controller?

scenario i’m imagining:

  1. I press the button

1a) if i’m still holding button before entering, code enters Sub::handle_jsbutton_press with held as true
1b) if i’m no longer holding button before entering, code enters Sub::handle_jsbutton_press with held as false

  1. since i mapped servo1_min_toggle, code enters k_servo_1_min_toggle switch case with the ‘held’ value from 1a or 1b

that’s what I seem to understand from looking at these if statements… but then I ask myself, what would that toggle function do if the ‘held’ boolean came in as true? because it seems like it would do nothing… so maybe Sub::handle_jsbutton_press is actually called twice? once when i first press the button, then again when i release the button?

I don’t completely understand what is going on here for this case.

50

So how should we be approaching the new code?

51

Was in the lab today trying to make more sense of the ESC’s behavior and here are my findings (similar to the previous findings but with more certainty now):

Code

  • I tried another while loop logic code, but once again, losing connection, so still not sure how the asynchronous update occurs. Still not sure whether I should be trying something with an if statement (and how), or if you have any idea how I can exit the while loop.

  • To troubleshoot, I made custom 3, 4 and 5 send pwm values of 1600, 1400 and 1500 respectively (no while logic, no if logic). This works (when the ESC is configured to 1500 as neutral, more on this in the next point). Here’s a video:

ESC Troubleshooting

  • We power everything and the ESC beeps 3 times. We open QGC and send a pwm value of 1500, the ESC makes a long beep, indicating that 1500 has been configured as the neutral position. From here, we operate just like the video. If any series of events from here on out results in 3 beeps and 1 long beep from ESC after reboot, then it is still configured to a neutral pwm of 1500.

  • In the case where I decided to configure neutral to be 1400, as expected, 1500 caused it to rotate, but then 1600 caused it to reset and configure 1600 as the new neutral position. We don’t know what the reason for this reset is, is it because the value of 1600 requires more amps than our regulator is providing or is it an issue with the ESC?

  • To return to 1500 being the neutral value, power off and on so that ESC beeps only 3 times, prompting for a new neutral input and send a value of 1500 for the long beep.

Other Question

We always have to power on the vehicle and THEN open QGC in order to connect to the vehicle. So in the case that we lose connection, we must always restart QGC in order to establish connection again; is this normal?

52

The suggestion is to match the implmentation of held with if statements, as demonstrated in other places in the file. This way, it will work (the code that is already there works, do the same thing and yours will too!). The way you have it right now, you enter the while loop, and you never come out of it, because the value of held never changes inside of the loop. (connection is lost because the code running on the autopilot is completely hung, doing nothing spinning around inside of your while loop forever.

We always have to power on the vehicle and THEN open QGC in order to connect to the vehicle. So in the case that we lose connection, we must always restart QGC in order to establish connection again; is this normal?

This is not normal, it is probably related to your firmware bugs.

53

Thanks for the reply!

The issue that I have preventing me from going forward with the if statement is not knowing exactly what is happening. In the toggle switch case, does held come in as ‘false’ after the user has pressed the button but not held it?

And so held would come in as true if the user has pressed the button and is still holding it?

And if that is indeed the case, then the code would be something like:

{
if (held) {
// do stuff to make motors spin
}
}

but how do I make it so that it goes back to neutral once the user is no longer holding the button? I can’t do this in the same switch case or else every time the user does hold the button, the duration of the motor running would basically be 0 (however fast the code is running)

54

I think we’ll just go with 3 buttons then instead of trying to implement for 2, I don’t see how I’d be able to do what I want to do if held doesn’t update to reflect current status.

That’ll just leave the issue of figuring out these ESC’s.

55

I think what you are trying to do is similar to the servo_n_max_momentary function. You should add a handler for press, and release.

You may not need to use the held variable after all, it is normally used to actually check if a button has not been held.

56

Ohh, thanks for mentioning the handler for release, I actually never noticed this. It was something I was curious about, initially thinking somehow the handler for press was being called twice with held as true and then held as false…

I am going to write something now using both handlers! I’ll be adding the custom switch cases into the release handler so that they’re called when the button is released.

57

Tested the new buttons today using press and release handler, working as intended, thanks!

We also tested our motors/ESC with the new voltage regulator and found that the “resetting to a different neutral value” actually occurs when the motor attempts to draw a current beyond the limit et by regulator. We were able to confirm this because our new regulator has a variable constant current that we were able to modify.

We will be looking into this more next week.

1 Like
58

Very good @adaniel1. I hope the rest goes more smoothly.

59

Hey, we’re currently doing some wet tests and troubleshooting movement issues.

Is the “Feedback control and stability” feature automatically enabled or must we change a parameter value within QGroundControl?

60

Hi @adaniel1, it is automatically enabled in stabilize and depth hold flight mode.
https://www.ardusub.com/operators-manual/flight-modes.html

61

Ah, we have been booting in Manual mode, which according to the link, means we had nothing enabled. Or does Manual mode include stabilize and depth hold?