Page 2 of 2 FirstFirst 12
Results 11 to 18 of 18

Thread: PhatomX with XL430-W250 servos

  1. #11

    Re: PhatomX with XL430-W250 servos

    @jwatte - The RC receiver arrived. Now to figure out how to set up the receiver for IBUS. So far I have seen a video looking like you hook up wire horizontal pins 4-6 (https://blog.georgi-yanev.com/fpv/se...ky-fs-i6-ibus/)

    And a few others...

    Probably simple enough to experiment, but any suggestions would be appreciated

  2. #12

    Re: PhatomX with XL430-W250 servos

    Yes, that's correct! You should put in 3.3V to 5V on the red wire, ground on the black wire, and you will get out UART at 115200 bps, 8 bit bytes, 1 stop bit, no parity.
    The protocol itself is easy to decode; the code I linked to above is free for use if you want to just steal it.

  3. #13

    Re: PhatomX with XL430-W250 servos

    Thanks @jwatte,

    I know the receiver and transmitter are talking now as when I plugged it in to 3.3v, the main unit complained of low battery... And the receiver had on it marked 4 to 8.4v.. So I plugged it into 5v from OpenCM board and it battery level showed better.

    I tried a quick and dirty sketch using your header file, which is not showing anything yet... Will debug. Still learning... Like if I need to set anything to get the transmitter to start outputting...

    Edit: Looks like wire was not correct... Now getting data on the first 4 channels... for the two joysticks. Probably now need to configure radio to output the buttons and knobs...

    Edit 2: I edited the Aux channels output, turned off output model so
    Channel 5-10: VRA, VRB, SWA, SWB, SWC, SWD
    Last edited by KurtEck; 3 Weeks Ago at 11:25 AM.

  4. #14

    Re: PhatomX with XL430-W250 servos

    @jwatte - Thanks again.

    Curious have you played at all with the bidirectional communications? I know you mentioned they are pretty limited on what information you can return,

    I saw saw some information up on: https://github.com/betaflight/betafl...Bus)-telemetry

    If I hook up logic analyzer, as soon as I apply power to the FS-IA6B I start to get messages on SENS pin...
    About every 7.7ms I get a message on that pin: 0x04, 0xF0, 0x0B 0xFF
    That repeats about 10 times, then there was a gap of about half second:

    Then I see the 0x04, 0x81, 0x7a, 0xff which matches the blog for something like: Sensor 1 are you there...
    Which appears to appear now every 7.7ms...

    Now when I turn on the transmitter: I continue to see this message on the SENS side, and the SERVO side I see the data messages.
    These message don't appear to overlap so on the blog he has them both connected to same Serial pin... Using resistor and diode...

    Not sure yet how I would want to experiment with this on OpenCM9 board, especially if the idea is to make it easy for others to do it hopefully without other parts and soldering...

    Now with the T4 beta board with something like 8 Uarts and the ability to add more using Flex IO pins... No brainer

  5. #15

    Re: PhatomX with XL430-W250 servos

    No, I have not played with the data sendback. I look forward to hearing what you find!

  6. #16

    Re: PhatomX with XL430-W250 servos

    Thanks, I will play with this some.

    Trying to decide how I will do it on OpenCM board. For a test case I might hook up SENS line to Serial1 (DXL ttl pin), which is more or less free if I use an OpenCM485 board for the servos, and is setup already for half duplex communications.

    My guess is it may not be worth adding extra hardware, but will try at least to see if it can get and display something.

  7. #17

    Re: PhatomX with XL430-W250 servos

    Quick update: Actually more notes to myself, @jwatte (and others).

    I now have a hacked up sketch, with a second hacked up copy of your flyskybus.h file, which I have hacked up to run on Serial1 on OpenCM board, using the half duplex support. It has a call back function that allows you to define extra field(s) and the like:
    Code:
    #if !defined(FlySkyIBusSENS_h)
    #define FlySkyIBusSENS_h
    
    #define IBUS_BAUDRATE 115200
    
    enum {FLYSENS_DISCOVER, FLYSENS_TYPE, FLYSENS_VALUE};
    typedef uint16_t (*FlySensCB)(uint8_t cb_type, uint8_t sens_index);
    
    class FlySkyIBusSENS {
      public:
        FlySkyIBusSENS(UARTClass &hws, FlySensCB  callback)
          : serial_(hws)
          , packetPtr_(0)
          , serialInTime_(0)
          , callback_(callback)
        {
        };
    
        void begin()
        {
          // Assume Serial 1
          Serial.println("FlySkyIBusSENS::begin called");
          serial_.setDxlMode(true);
          drv_dxl_begin(0);
          serial_.begin(IBUS_BAUDRATE);
          drv_dxl_tx_enable(0, false);  // put in read mode.
        }
    
        void step()
        {
          if (!serial_.available()) return;   // nothing to do.
          while (serial_.available()) {
            int i = serial_.read();
            //Serial.print(".");
            if (((millis() - serialInTime_) > 3) || (packetPtr_ >= sizeof(packet_))) packetPtr_ = 0;  // Assume any delay is we are not aligned
                packet_[packetPtr_++] = i;  // save away the character.
            if (packetPtr_ == packet_[0]) {
              if (serial_.available()) digitalWrite(BOARD_LED_PIN, !digitalRead(BOARD_LED_PIN));
              // in theory we have a complete packet.
              // Does checksum match?
              uint8_t packet_size = packet_[0];
                uint16_t checksum_in = ((uint16_t)packet_[packet_size - 1] << 8) + packet_[packet_size - 2];
                uint16_t checksum_calc = 0xffff;
                for (uint8_t ib = 0; ib < (packet_size - 2); ib++) checksum_calc -= packet_[ib];
                if (checksum_in == checksum_calc) {
                  // have a packet.  What type is it?
                  if ((packet_[1] & 0xf0) == 0x80) {
                    // Discovery packet
                    if ((*callback_)(FLYSENS_DISCOVER, packet_[1] & 0x0f)) {
                      // It says we want to handle this device.
                      drv_dxl_tx_enable(0, true);  // put in write mode.
                      serial_.write(packet_, packet_size);  // echo the packet back
                      serial_.flush();  // wait until it outputs
                      drv_dxl_tx_enable(0, false);  // put back in Read mode
                    }
    
                  } else if ((packet_[1] & 0xf0) == 0x90) {
                    // What type of packet do we have?
                    uint8_t sensor_type = (*callback_)(FLYSENS_TYPE, packet_[1] & 0x0f);
                    // It says we want to handle this device.
                    drv_dxl_tx_enable(0, true);  // put in write mode.
                    // Lets generate a return packet.
                    checksum_calc = 0xffff - (6 + 2);
                    serial_.write(6);             // packet size
                    serial_.write(packet_[1]);    // packet type and sensor number
                    checksum_calc -= packet_[1];
                    serial_.write(sensor_type);  // sensor type from call back
                    checksum_calc -= sensor_type;
                    serial_.write(2);             // always 2? - maybe data size?
                    serial_.write(checksum_calc & 0xff); // low byte checksum
                    serial_.write(checksum_calc >> 8);  // high byte checksum
                    serial_.flush();  // wait until it outputs
                    drv_dxl_tx_enable(0, false);  // put back in Read mode
                  } else if ((packet_[1] & 0xf0) == 0xA0) {
                    // Return data.
                    uint16_t sensor_data = (*callback_)(FLYSENS_VALUE, packet_[1] & 0x0f);
                    // It says we want to handle this device.
                    drv_dxl_tx_enable(0, true);  // put in write mode.
                    // Lets generate a return packet.
                    checksum_calc = 0xffff - 6;
                    serial_.write(6);             // packet size
                    serial_.write(packet_[1]);    // packet type and sensor number
                    checksum_calc -= packet_[1];
                    serial_.write(sensor_data & 0xff);  // sensor type from call back
                    checksum_calc -= (sensor_data & 0xff);
                    serial_.write(sensor_data >> 8);       // always 2? - maybe data size?
                    checksum_calc -= (sensor_data >> 8);
                    serial_.write(checksum_calc & 0xff); // low byte checksum
                    serial_.write(checksum_calc >> 8);  // high byte checksum
                    serial_.flush();  // wait until it outputs
                    drv_dxl_tx_enable(0, false);  // put back in Read mode
                  }
                } else {
                  Serial.println("Checksum error");
                }
                packetPtr_ = 0;
              }
          }
          serialInTime_ = millis();
        }
    
      private:
    
        UARTClass &serial_;
        uint8_t       packetPtr_;
        uint8_t       packet_[10];
        uint32_t      serialInTime_;
        FlySensCB     callback_;
    };
    
    #endif
    The previous link did not give much information on what the field type(s) were, so played a little. Currently by call back function looks pretty simple:
    Code:
    uint16_t cb_value = 0;
    uint16_t sense_callback(uint8_t cb_type, uint8_t cb_index) {
      if (cb_index != 1) return 0;
      switch (cb_type) {
        case FLYSENS_DISCOVER:
          return 1;
        case FLYSENS_TYPE:
          return 2; // not sure yet.
        case FLYSENS_VALUE:
          cb_value += 10; // not sure of range yet.
          return cb_value;
      }
    }
    So adds one field of type 2... and increments it by 10 each time called.

    For awhile I did not think it was doing anything... Actually took me awhile to figure out how to get any fields like this to display on the screen. Finally looked through PDF manual... You need to swipe the display right to left...

    After I did that it was showing a few fields including one with ID 1 of type Motor RPM, which was going all over everywhere... So looks like the data is getting there.

    Did some more searching and found, this github project: https://github.com/Yenya/ibus-altitude-sensor,
    which had source file with types of data:
    Code:
    #define IBUS_SENS_INTV    0x00 // Internal Voltage
    #define IBUS_SENS_TEMP    0x01 // Temperature
    #define IBUS_SENS_RPM   0x02 // RPM
    #define IBUS_SENS_EXTV    0x03 // External Voltage
    #define IBUS_SENS_CLIMB   0x09 // Clibm rate m/s *100
    #define IBUS_SENS_PRES    0x41 // Pressure
    #define IBUS_SENS_GPS_ALT 0x82 // 4 bytes signed GPS alt m*100
    #define IBUS_SENS_ALT   0x83 // 4 bytes signed Alt m*100
    #define IBUS_SENS_ALT_MAX 0x84 // 4 bytes signed Alt m*100
    So RPM display appears to match...

    So will probably play some more and see what happens if I send external voltage... Might be nice if it showed up on main display...

  8. Re: PhatomX with XL430-W250 servos

    I've found the OpenCM904 to be a good substitute for the Arbotix and I'm controlling the Hexapod with the Phoenix code using the Commander with a Xbee connected to one of the serial ports on the OpenCM904. I've also connected a LidarLite on the turret to the OpenCM904 via PWM. Works fine. Also connected a Pixy2 to the OpenCM904 via I2C. Also works fine.
    We've added code to the _Phoenix_Input_Commander.h which reads the sensors and controls the robot by changing the g_InControlState variables. This also works fine but looping through the code means that sensors aren't sampling fast enough for the robot to be really responsive while it's walking. I really like the kinematics of the Phoenix code and don't want to change it.
    So I have mounted a RPI0 on the turret to drive a PICAMERA. It then streams the video using MJPG-streamer via the RPI0 WIFI connection. So I can get a streaming video robot eye view on any browser on the same WIFI network.
    Given all this I'm now wondering if it would make sense to connect all the sensors to the RPI0 and write code that runs on the RPI0 and controls the Hexapod either via a wired connection (or Xbee as I have 2 Xbees already mounted on the Hexapod)
    As a beginner with all of this I find the software the most challenging. My son has been helping with the C but his time is limited and it's not a language I'm expert with. There's a lot more help around to configure camera and sensor connections on the RP and plenty of examples in Python on SLAM.
    So my question is would it make sense to control the Hexapod running Phoenix code from a RPI mounted on the Hexapod? it's a much cheaper option than running the more expensive OpenCM boards and the RPI0 has low power consumption.

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Similar Threads

  1. XL430-W250 Stop moving randomly
    By Larcay in forum Software and Programming
    Replies: 6
    Last Post: 02-15-2019, 03:03 PM
  2. New XL430-W250-T DOES NOT MOVE
    By Larcay in forum DYNAMIXEL & Robot Actuators
    Replies: 5
    Last Post: 09-14-2018, 11:54 AM
  3. News/Announcement Beta testing XL430
    By Zenta in forum DYNAMIXEL & Robot Actuators
    Replies: 6
    Last Post: 04-22-2017, 11:55 AM
  4. Replies: 4
    Last Post: 12-09-2014, 09:01 AM
  5. HITEC Servos: Can the OEM RN-1 Servos handle 7.4V?
    By MYKL in forum DYNAMIXEL & Robot Actuators
    Replies: 10
    Last Post: 06-23-2008, 04:39 PM

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •