91 Individual Droid: D‐O
Mot1
1 VCC —> Blue —> Pololu L VCC
2 GND —> Green —> Pololu L GND
3 PinA1/D16(I) MOT1_ENCA —> Yellow —> Pololu L ENCA
4 PinA2/D17(I) MOT1_ENCB —> White —> Pololu L ENCB
5 PinA0/D15 MOT1_EN —> White —> DRV8871 L GND (pull low)
6 PinA3/D18~ MOT1_PWMA —> Red —> DRV8871 L IN1
7 PinA4/D19~ MOT1_PWMB —> Black —> DRV8871 L IN2
Mot2
1 VCC —> Blue —> Pololu R VCC
2 GND —> Green —> Pololu R GND
3 PinD7~(I) MOT2_ENCA —> Yellow —> Pololu R ENCA
4 PinD6~(I) MOT2_ENCB —> White —> Pololu L ENCB
5 PinA5/D20 MOT2_EN —> White —> DRV8871 R GND (pull low)
6 PinD2~ MOT2_PWMA —> Red —> DRV8871 R IN1
7 PinD3~ MOT2_PWMB —> Black —> DRV8871 R IN2
10-Pin Head Connector
Pin 1 STEMMA GND (Black) / Dynamixel GND (Black) / USB GND
Pin 2 STEMMA V+ (Red)
Pin 3 STEMMA SDA (Blue)
Pin 4 STEMMA SCL (Yellow)
Pin 5 Dynamixel V+ (Middle) —> Green
Pin 6 Dynamixel TTL (Non-Striped) —> Red
Pin 7-10 Unused (Neopixel?)
Dynamically unstable droids like D-O depend on a couple of things to be correct before their control algorithms can work. What the control algorithm essentially does is control the droid's counterweight pitch against a setpoint, actuating the motors to achieve this and measuring pitch with the IMU. If both motors are reversed, the droid will accelerate in the wrong direction until something stops it; if the one is reversed, it will turn quicker and quicker; if the IMU is rotated there is no pitch change so the control algorithm is useless and the droid will probably not move at all, etc.
To make sure all systems are aligned correctly, D-O has a selftest method that is called at every startup, or can be triggered manually by the d-o selftest command in the console. For the selftest, D-O needs a bit of room!
The routine works as follows:
- Make sure IMU and current meter are available and responding.
- Slowly drive all connected servos to their normal position; this means neck upright, head horizontal and pointing to the front.
- Accelerate the left motor gradually forward up to a certain maximum PWM (no speed control here, this is raw PWM). This will at some point cause the droid to pivot around the right wheel!
- Measure the following, and operate on the measurement:
- drawn current (from the current meter), and if current is too high stop accelerating immediately;
- acceleration (from the IMU), and if a certain acceleration was reached stop accelerating immediately;
- distance driven by the left wheel (from the encoder), and if a certain distance was reached stop accelerating immediately;
- If not aborted by one of the above criteria, stop accelerating when maximum PWM is reached.
- Gradually decelerate the left motor back to 0.
- Use the measured values for diagnostics, and return appropriate error messages:
- If current too high, the motor is blocked.
- If high acceleration is measured along the y axis, the IMU is turned by 90°.
- If the encoder does not report enough distance driven,
- but sufficient acceleration was measured, the encoder is likely disconnected.
- and insufficient acceleration was measured, the motor is likely disconnected.
- If the encoder reports enough distance driven but insufficient acceleration was measured, the droid is probably stationary in its cradle (i.e. the motors turn freely but the droid does not move). This is no reason to abort but it means we cannot distinguish wrong motor direction and wrong encoder direction in the next step.
- If the motor PWM direction and the direction of the driven distance disagree,
- but acceleration was measured in the direction of the motor PWM, the encoder leads are likely reversed.
- and acceleration was also measured opposing the direction of the motor PWM, the motor leads are likely reversed.
- If we are still here, we have ascertained that
- the motor is not blocked
- the IMU is oriented correctly
- encoder and motor are connected and driving in the same direction as the IMU measures acceleration.
- We also know if the droid is driving freely or is in its station.
- Repeat this by running the right motor, but backward (so if everything goes well the droid has turned a bit in place but not moved much).
A PID controller is used to control the droid's pitch, with the setpoint set to 0 and measurement from the IMU. The speed and turn signal from the remote are added to the output of this PID controller, creating individual wheel setpoints for a PID speed controller for each drive wheel, which are responsible to regulate away friction differences. This allows for slow driving and handling different ground structures.
FIXME Different speed control parameters for different ground surfaces?
This is currently a theoretical concept. We have an IMU to measure turn rate, and we can use it to control the droid's turn. In this mode, we do not give direct speed differences from the remote to each wheel, but the remote governs the yaw rate set point. A PID controller then turns this into differential signals for both speed-controlled wheels. This mode is something I want to try, but whether it will make any difference for the positive is doubtful. It will be sensitive to IMU heading drift, and since we have no compass correcting for it will be quite involved, like an extended Kalman filter taking encoder motion into accound.
IMU balancing with a PID controller always has the disadvantage that it will overshoot with sudden manoeuvers, because the controller is always lagging about a half wave behind. Acceleration settings and feedforward will be able to compensate this to a degree, but probably not 100%. Maybe it would be a good idea to move to MPC or other advanced control concepts and leave the simple PID behind.
- NEVER plug/unplug the neck or any neck components while the droid is switched on / has power. I have fried multiple MKR1010s doing this until I realized.