Servo Modes - Basic

[danic85]

I need a way to be able to change the various 'modes' of the legs (or servos) depending on the situation.

This is a precursor to the robot detecting this itself, but in the meantime there are a few different modes I'd like to be able to select during operation:

• Disabled: No servos can move from the initial position. Useful for software upgrades and development.
• Sit: Head can animate, but legs are fixed in seated position. I use this position most often as it is more stable.
• Stand: Head can animate and legs stand after starting in a seated position. MPU6050 IMU data is used to balance body.
• Backpack: Head animates. Robot can be mounted to backpack, legs are position to assist with this. No balancing takes place.
• Straight: The head animates but the legs are straight. The used for carrying in a backpack.

There is also a restraining bolt mode, which should keep the robot in a seated position. This may be removed in future.

Finally, the calibration script must still be accessible. (Initially this can be from the single 'stand' mode, but ideally it should work for all.)

To facilitate this I have added a legMode to the arduino sketch that will read analog input from pin A1 after boot and determine which mode, based on a series of resistor values:

// config.h
int servoModePin = PIN_A1;
// Define 5 threshold values between 0 and 1024 for the 5 leg modes
int servoModeThresholds[5] = {205, 410, 615, 820, 1024};
String servoModeNames[] = {"Disabled", "Sit", "Stand", "BackPack", "Straight"};
int *servoModePoses[] = {PosStart, PosStart, PosStart, PosBackpack, PosStraight};

// ServoManager.h

 void initInputs()
    {
        #ifdef SERVO_MODE_PIN_ENABLED
            pinMode(servoModePin, INPUT); // sets the digital pin as input
            int servoModeVal = analogRead(servoModePin);
            for (int i = 0; i < 5; i++)
            {
                if (servoModeVal < servoModeThresholds[i])
                {
                    servoMode = i;
                    break;
                }
            }
            // Serial.print("Servo mode: ");
            // Serial.println(servoMode);
            #ifdef SERVO_MODE_OVERRIDE
            servoMode = SERVO_MODE_OVERRIDE; // Define and set in Config.h if appropriate
            // Serial.print("Servo mode override: ");
            // Serial.println(servoMode);
            #endif
            
            StartingPos = servoModePoses[servoMode];
        #endif

        #ifdef RESTRAIN_PIN_ENABLED
            pinMode(restrainPin, INPUT_PULLUP); // sets the digital pin as input
            if (digitalRead(restrainPin) == LOW) // Check once on startup
            {
                restrainingBolt = true;
                StartingPos = PosStart; // Override starting position @todo remove once selector is installed
            }
        #endif
    }

This is simple enough to override, by bypassing the above method and setting StartingPos directly.

[danic85]

I have created PR #128 for this.

[danic85]

This has been merged to develop as the functionality has been tested. It can be disabled by commenting out the SERVO_MODE_PIN_ENABLED define in Config.h