search

FTC-9974-THOR / ThorCore

A library for FTC
Other
4 stars 2 forks source link

Pixy2 #1

Open Team14423 opened 3 years ago

Team14423 commented 3 years ago

Our team has been looking for a solution to the I2cDeviceSynch registers problem, and was intrigued by your apparent solution, to bypass it and send directly. They implemented the code tonight, and got an error that our driver did not have a necessary constructor. when super() is being called in the constructor, it seems like it wants the 3 parameter constructor of LynxDeviceSynchEx, and not the two parameter constructor of I2cDeviceSynchDevice.

So, we were clever - we decided to do used i2cDeviceSynchDevice for our hardwaremap class, and then cast it in the super() constructor as (LynxI2CDeviceSynchEx). It compiled fine, but now the robot says it can't find a device with our instancename of type I2cDeviceSynch in the configuration (which makes sense, obviously, since the driver is of type LynxI2CDeviceSynchEx.

So, we figure it's one of two things:

  1. We set it up wrong
  2. You didn't get this to work

I'm hoping it is No. 1. We think this idea has legs, but we're just missing something in how to get the constructor write given the way the inheritance is set up here.

Edited to add a third option: the SDK changed. It looks like I2cDeviceSimple used to inherit from LynxI2cDeviceSynch, but doesn't any longer, and that's what's causing our type mismatches.

fortraan commented 3 years ago

Hey, I just saw this issue today. Back when I wrote the Pixy2 class, I got stuck on the constructor problem. I couldn't find a way to get the SDK to be happy with LynxDeviceSynchEx instead of I2cDeviceSynch. In the end, I didn't use the Pixy on the robot, so I never finished the Pixy2 class. Today I took a look at the problem again, and I found a solution that might work.

According to the documentation in the class I2cDeviceType, the code in the SDK that instantiates I2C devices will only work with classes that have a constructor matching at least one of these signatures:

As far as I can tell, there's no way to get it to allow Pixy2's constructor signature (ctor(LynxI2cDeviceSynchEx device)). However, there may be a way to work around this. The only thing needed to send commands to a LynxModule is just an instance of the LynxModule itself, which can be obtained from the hardware map. If I can get hardwareMap from an OpMode to Pixy2, I think I can get the LynxModule instance and send it commands directly. If this works, then I can bypass the constructor problem entirely.

I'm going to work on this, and I'll give you an update once I've figured it out.

Team14423 commented 3 years ago

Thanks for getting back. I wondered about the constructor issue. I thought maybe you had written this earlier, when I2cDeviceSimple used to implement LynxI2cDeviceSynch (it doesn't any longer).

We decided to just use the analog/y port, which works fine for our needs, but I would love to get to the bottom of this, so please do report back if you can figure out how to make it work. We are just not familiar enough with the SDK to do it (though rapidly getting there based on how much I've studied the last week or so!)

fortraan commented 3 years ago

I've solved the constructor problem. By making Pixy2 extend I2CDeviceSynchDeviceWithParameters, I can have it accept an initialization parameter. Since initialization is done in an OpMode, I can get the LynxModule the Pixy is connected to and pass it to the Pixy2 instance. From there, I can send commands directly to the module. With this, I've been able to communicate with the Pixy.

Writing to I2C seems to be working correctly. The Pixy's SET_RGB_LED and SET_LAMP commands both work, but they're also the only 2 "write" commands I've tried so far. "Read" commands, however, aren't working properly. The issues so far are:

So far, I've tried GET_VERSION and GET_RESOLUTION. GET_VERSION seems to work, but the first letter of the firmwareType String is corrupted. GET_RESOLUTION just returns garbage data.

There's also some interesting behavior:

I'm powering the Pixy with the 5V auxillary port on an Expansion Hub, and the I2C lines (SDA and SCL) are connected directly to the I2C lines of I2C port 0 on the Hub. I'm worried this wiring might be an issue, because the Pixy is measuring SDA and SCL relative to the 5V port's ground. I don't know if I2C ground is connected to 5V ground within the Expansion Hub, so SDA and SCL might be floating pins for all the Pixy can tell. I've also had problems with Expansion Hub I2C in the past. Last season, I found that the Expansion Hub's I2C lines didn't transition from 0V to 3.3V fast enough, so I had to add external pullup resistors. I don't have these resistors connected right now, so that might be part of the problem.

I made a dev branch and put the current code in there. Pixy2 is here: https://github.com/FTC-9974-THOR/ThorCore/blob/dev/src/main/java/org/ftc9974/thorcore/robot/sensors/Pixy2.java

In summary, communicating with the Pixy over I2C is possible. However, there's still problems to work out before it can be used in competition.

fortraan commented 3 years ago

Here's the OpMode I've been using for testing. pixy.initialize(LynxModule module, int bus) needs the LynxModule the Pixy is connected to and the port it's connected to. Here I had it connected to an Expansion Hub on port 0, so the device name in the hardware map was "Expansion Hub 3" and bus is 0. The way I handled the bus parameter in Pixy2 is super hacky, but it's only a temporary solution until I make a proper parameters class like what BNO055IMU uses.


package org.firstinspires.ftc.teamcode;

import android.graphics.Color;

import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.ftc9974.thorcore.control.math.Vector2;
import org.ftc9974.thorcore.internal.CommunicationException;
import org.ftc9974.thorcore.robot.sensors.Pixy2;
import org.ftc9974.thorcore.util.MathUtilities;

@TeleOp(name = "Pixy2 Test")
public class Pixy2TestOpMode extends OpMode {

    private Pixy2 pixy;

    private ElapsedTime timer;

    @Override
    public void init() {
        pixy = hardwareMap.get(Pixy2.class, "pixy");
        pixy.initialize(hardwareMap.get(LynxModule.class, "Expansion Hub 3"), 0);

        try {
            Pixy2.Version version = pixy.getPixyVersion();
            telemetry.addData("Major", version.majorFirmware);
            telemetry.addData("Minor", version.minorFirmware);
            telemetry.addData("Build", version.firmwareBuild);
            telemetry.addData("Hardware", version.hardwareVersion);
            telemetry.addData("Type", version.firmwareType);

            pixy.setLamp(true, true);

            Vector2 resolution = pixy.getResolution();
            telemetry.addData("Resolution", resolution.toString());
        } catch (CommunicationException e) {
            telemetry.addData("Failed", e.getMessage());
            telemetry.update();
        }

        timer = new ElapsedTime();
    }

    @Override
    public void start() {
        try {
            pixy.setLamp(false, false);
        } catch (CommunicationException e) {
            telemetry.addData("Failed", e.getMessage());
            telemetry.update();
        }
        timer.reset();
    }

    @Override
    public void loop() {
        float[] hsv = {
                (float) MathUtilities.map(timer.seconds(), 0, 2, 0, 360),
                1f,
                1,
        };
        if (hsv[0] > 360) {
            timer.reset();
        }
        telemetry.addData("Time", timer.seconds());
        int color = Color.HSVToColor(hsv);
        try {
            pixy.setLED(Color.red(color), Color.green(color), Color.blue(color));
        } catch (CommunicationException e) {
            telemetry.log().add(e.getMessage());
        }
    }
}```
Team14423 commented 3 years ago

This is tremendous progress. Thank you! We use a level shifter - maybe that will help. We've got to get going for competition, so this may have to move to side project, but it's on the way! The Pixy2 source code is available - I was studying it the other day - it's pretty helpful in figuring out what it's supposed to be sending back. I had hoped to trace how it uses the register field, and had a workaround for the write, but the read wasn't going to work, because they don't deal with the write of the register address (which led to the timeout). But that may show why there's mangling of the first byte in getVersion

fortraan commented 3 years ago

Hey, it's been a while. School started back up for me a while ago, and it's been keeping me really busy since then. Over the last 2 weekends I've finally gotten time to work on the Pixy again. I noticed a loophole in the rules that allows the Pixy to be connected to the REV Control Hub by USB instead of I2C, so I got libpixy2usb running on the Control Hub. It works nearly perfectly, allowing access to all features of the camera. There are however some major problems, namely the Control Hub OS denying read access to USB devices for seemingly no apparent reason and making prebuilt native libraries for ThorCore so it can be compiled without having clang, an assembler, and the Android NDK installed (this is why I haven't pushed the newest version to git; the buildscript isn't very stable right now and probably wouldn't work on any other computer). But once those are cleared up, it looks like it should work flawlessly.

Wednesday is probably the soonest I'll have free time, so I hope to work on it then, as well as write up a more detailed update.

Team14423 commented 3 years ago

This is really great stuff.

fortraan commented 3 years ago

I've pushed the new code to the dev branch. The 2 classes of interest are Pixy2USB and Pixy2USBManager.

Most of the new code is written in C++, so you'll need the Android NDK and CMake to build it. Android Studio should be able to automatically download and configure those, but I've had problems with it in the past.

There's a loophole in the rules concerning sensors on the Expansion and Control Hub - it doesn't specify what ports are allowed, so USB is fair game. Unfortunately this loophole does not apply to the phones, only the Control Hub.

I packaged libpixyusb2 in native code and built a JNI wrapper around it so I could interface with the Pixy. Due to how Android handles permissions, the Java side is responsible for opening and closing USB connections, as well as listening for connection events. The native code just wraps a libusb handle around the file descriptor of the USB connection.

For some reason, Android really didn't like accessing USB from native code. It kept denying permissions in C++, even after granting them in Java. I still haven't completely fixed the problem, but I have figured out how to work around it for now. The PIxy2USBManager can't detect Pixies if they were plugged in before the robot controller app started (ie, if they were plugged in before the robot was turned on). However, you can simply unplug and plug back in the Pixy while the robot's turned on, and it will fix the problem.

To actually interface with a Pixy, get a list of connected Pixies from Pixy2USBManager. I've yet to find a way to uniquely identify them that persists through power cycles, unfortunately. Pixy2USB objects expose API methods for the physical Pixy they represent, and teardown is handled automatically. I put together a sample OpMode here to demonstrate what I have implemented so far.

Team14423 commented 3 years ago

We will give this a try. This is really impressive work.