sl-firmware
e4116dffc0
Enable Jetson autonomous arming while keeping RC as optional override. Changes: - RC kill switch (CH5 OFF) now triggers emergency stop instead of disarm → Allows Jetson-armed robots to remain armed when RC disconnects → Maintains kill switch safety for emergency situations - RC disarm only triggers on explicit CH5 falling edge (RC still alive) → RC disconnect doesn't disarm Jetson-controlled missions → RC failsafe timer (500ms) handles signal loss separately - Jetson arming via CDC 'A' command works independently of RC state → Robots can operate fully autonomous without RC transmitter → Heartbeat timeout (500ms) prevents runaway if Jetson crashes Safety maintained: - Arming hold timer: 500ms (prevents accidental arm) - Tilt limit: ±10° level required - IMU calibration: Required before any arm attempt - Remote E-stop: Blocks all arming - RC failsafe: 500ms signal loss = disarm - Jetson timeout: 500ms heartbeat = zero motors Command protocol (unchanged): - Jetson: A=arm, D=disarm, E=estop, Z=clear estop - RC: CH5 switch (optional override) - Heartbeat: H command every ≤500ms - Drive: C<speed>,<steer> every ≤200ms See AUTONOMOUS_ARMING.md for complete protocol and testing checklist. Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
178 lines
5.3 KiB
Markdown
178 lines
5.3 KiB
Markdown
# Motor Test Joystick (Issue #513)
|
||
|
||
Terminal-based interactive joystick for bench testing SaltyBot motors via Termux.
|
||
|
||
## Quick Start
|
||
|
||
On phone (Termux):
|
||
```bash
|
||
# With ROS2 (default, requires ros_core running on Jetson)
|
||
python3 ~/saltylab-firmware/phone/motor_test_joystick.py
|
||
|
||
# With WebSocket (if ROS2 unavailable)
|
||
python3 ~/saltylab-firmware/phone/motor_test_joystick.py --backend websocket --host <jetson-ip>
|
||
```
|
||
|
||
## Controls
|
||
|
||
| Key | Action |
|
||
|-----|--------|
|
||
| **W** / **↑** | Forward (linear +X) |
|
||
| **S** / **↓** | Reverse (linear -X) |
|
||
| **A** / **←** | Turn left (angular +Z) |
|
||
| **D** / **→** | Turn right (angular -Z) |
|
||
| **SPACE** | E-stop toggle (hold disables motors) |
|
||
| **R** | Reset velocities to zero |
|
||
| **Q** | Quit |
|
||
|
||
## Features
|
||
|
||
### Real-Time Feedback
|
||
- Live velocity displays (linear X, angular Z)
|
||
- Velocity bar graphs with ● indicator
|
||
- Current input state (before clamping)
|
||
- Timeout warning (>500ms since last command)
|
||
- Status message line
|
||
|
||
### Safety Features
|
||
- **E-stop button** (spacebar): Instantly zeros velocity, toggle on/off
|
||
- **Timeout safety**: 500ms without command → sends zero velocity
|
||
- **Velocity ramping**: Input decays exponentially (95% per frame)
|
||
- **Conservative defaults**: 0.1 m/s linear, 0.3 rad/s angular
|
||
- **Graceful fallback**: WebSocket if ROS2 unavailable
|
||
|
||
### Dual Backend Support
|
||
- **ROS2 (primary)**: Publishes directly to `/cmd_vel` topic
|
||
- **WebSocket (fallback)**: JSON messages to Jetson bridge (port 9090)
|
||
|
||
## Usage Examples
|
||
|
||
### Standard ROS2 mode (Jetson has ros_core)
|
||
```bash
|
||
python3 motor_test_joystick.py
|
||
```
|
||
Sends Twist messages to `/cmd_vel` at ~20Hz
|
||
|
||
### WebSocket mode (fallback if no ROS2)
|
||
```bash
|
||
python3 motor_test_joystick.py --backend websocket --host 192.168.1.100
|
||
```
|
||
Sends JSON: `{"type": "twist", "linear_x": 0.05, "angular_z": 0.0, "timestamp": 1234567890}`
|
||
|
||
### Custom velocity limits
|
||
```bash
|
||
python3 motor_test_joystick.py --linear-max 0.5 --angular-max 1.0
|
||
```
|
||
Max forward: 0.5 m/s, max turn: 1.0 rad/s
|
||
|
||
### Combine options
|
||
```bash
|
||
python3 motor_test_joystick.py \
|
||
--backend websocket \
|
||
--host saltybot.local \
|
||
--linear-max 0.2 \
|
||
--angular-max 0.5
|
||
```
|
||
|
||
## Architecture
|
||
|
||
### MotorTestController
|
||
Main state machine:
|
||
- Manages velocity state (linear_x, angular_z)
|
||
- Handles e-stop state
|
||
- Enforces 500ms timeout
|
||
- Clamps velocities to max limits
|
||
- Sends commands to backend
|
||
|
||
### Backend Options
|
||
|
||
**ROS2Backend**: Direct Twist publisher
|
||
- Requires `geometry_msgs` / `rclpy`
|
||
- Topic: `/cmd_vel`
|
||
- Spin thread for ros2.spin()
|
||
|
||
**WebSocketBackend**: JSON over TCP socket
|
||
- No ROS2 dependencies
|
||
- Connects to Jetson:9090 (configurable)
|
||
- Fallback if ROS2 unavailable
|
||
|
||
### Curses UI
|
||
- Non-blocking input (getch timeout)
|
||
- 20Hz refresh rate
|
||
- Color-coded status (green=ok, red=estop/timeout, yellow=bars)
|
||
- Real-time velocity bars
|
||
- Exponential input decay (95% per frame)
|
||
|
||
## Terminal Requirements
|
||
|
||
- **Size**: Minimum 80×25 characters (larger is better for full feedback)
|
||
- **Colors**: 256-color support (curses.init_pair)
|
||
- **Non-blocking I/O**: ncurses.nodelay()
|
||
- **Unicode**: ● and 🛑 symbols (optional, falls back to ASCII)
|
||
|
||
### Test in Termux
|
||
```bash
|
||
stty size # Should show >= 25 lines
|
||
echo $TERM # Should be xterm-256color or similar
|
||
```
|
||
|
||
## Performance
|
||
|
||
| Metric | Value | Notes |
|
||
|--------|-------|-------|
|
||
| **UI Refresh** | 20 Hz | Non-blocking, timeout-based |
|
||
| **Command Rate** | 20 Hz | Updated per frame |
|
||
| **Timeout Safety** | 500ms | Zero velocity if no input |
|
||
| **Input Decay** | 95% per frame | Smooth ramp-down |
|
||
| **Max Linear** | 0.1 m/s (default) | Conservative for bench testing |
|
||
| **Max Angular** | 0.3 rad/s (default) | ~17°/s rotation |
|
||
|
||
## Troubleshooting
|
||
|
||
### "ROS2 module not found"
|
||
→ Run with `--backend websocket` instead
|
||
|
||
### "Connection refused" (WebSocket mode)
|
||
→ Check Jetson IP with `--host <ip>`, verify bridge listening on :9090
|
||
|
||
### Motors not responding
|
||
1. Check e-stop status (should show "✓ Inactive")
|
||
2. Verify timeout warning (>500ms = zero velocity sent)
|
||
3. Check Jetson `/cmd_vel` subscription: `ros2 topic echo /cmd_vel`
|
||
4. Verify network connectivity (WiFi/tethering)
|
||
|
||
### Terminal artifacts / display issues
|
||
- Try `reset` or `stty sane` in Termux
|
||
- Increase terminal size (pinch-zoom)
|
||
- Use `--backend websocket` (simpler UI fallback)
|
||
|
||
## Safety Checklist Before Testing
|
||
|
||
- [ ] Phone connected to Jetson (WiFi or USB tether)
|
||
- [ ] Motors disconnected or isolated (bench testing mode)
|
||
- [ ] E-stop accessible (spacebar, always reachable)
|
||
- [ ] Terminal window visible (no hide/scroll)
|
||
- [ ] Max velocities appropriate (start conservative: 0.1/0.3)
|
||
- [ ] Kill switch ready (Ctrl+C, or `ros2 topic pub --once /cmd_vel ...`)
|
||
|
||
## Future Enhancements
|
||
|
||
- [ ] Gamepad/joystick input (evdev) instead of keyboard
|
||
- [ ] Configurable button mappings
|
||
- [ ] Velocity profile presets (slow/medium/fast)
|
||
- [ ] Motor current feedback from motor driver
|
||
- [ ] Telemetry logging (CSV) for bench analysis
|
||
- [ ] Multi-motor independent control
|
||
|
||
## Related Issues
|
||
|
||
- **#420** — Termux bootstrap & Android phone node
|
||
- **#508** — Face LCD animations (separate system)
|
||
- **#512** — Autonomous arming (uses /cmd_vel via motor bridge)
|
||
|
||
## References
|
||
|
||
- [ROS2 Humble - geometry_msgs/Twist](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Different-Distributions.html)
|
||
- [Termux - Python environment](https://wiki.termux.com/wiki/Python)
|
||
- [ncurses - Curses module](https://docs.python.org/3/library/curses.html)
|