Results 1 to 4 of 4

Thread: Trying to add a gyro tot arbotix commander

  1. Trying to add a gyro tot arbotix commander

    So first of I am not really a programmer, more a tinkerer I would say. I was trying to add a gyro to the arbotix commander and for that I need it to send 3 extra value to the robot (Yaw, Pith Roll). There is already room for one extra value, but still I need 2 more. In my humble opinion the 2 parts of code that need to be changed are to ones below, one on the robot and one on the commander....i think. I tried to make all kinds of changes, but can't get it to work. I thought in essence it should look something like the way like shown in the code below, but I am very likely doing something wrong. In the calculation I use for the gyro it gives me the values within the desired range, same as the sticks. I did put the gyro part in another routine, but this way you can see what I am doing.

    Can anyone help me a bit in the right direction? Or is it not possible?


    Code:
    void loop(){
        if (!dmpReady) return;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            mpu.resetFIFO();
        } else if (mpuIntStatus & 0x02) {
            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
            mpu.getFIFOBytes(fifoBuffer, packetSize);
            fifoCount -= packetSize;
                mpu.dmpGetQuaternion(&q, fifoBuffer);
                mpu.dmpGetGravity(&gravity, &q);
                mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        }
    
        int right_V = (1023-analogRead(RIGHT_V)-512)/5 + 128;
        int right_H = (analogRead(RIGHT_H)-512)/5 + 128;
        int left_V = (1023-analogRead(LEFT_V)-512)/5 + 128;
        int left_H = (analogRead(LEFT_H)-512)/5 + 128;
        int Gyro_P = ((ypr[0] * 180/M_PI)/180 * 128) + 128;
        int Gyro_R = ((ypr[1] * 180/M_PI)/90 * 128) + 128;
        int Gyro_Y = ((ypr[2] * 180/M_PI)/90 * 128) + 128;
     
        //buttons =    
        unsigned char buttons = 0;
        if(digitalRead(BUT_R1) == LOW) buttons += 1;
        if(digitalRead(BUT_R2) == LOW) buttons += 2;
        if(digitalRead(BUT_R3) == LOW) buttons += 4;
        if(digitalRead(BUT_L4) == LOW) buttons += 8;
        if(digitalRead(BUT_L5) == LOW) buttons += 16;
        if(digitalRead(BUT_L6) == LOW) buttons += 32;
        if(digitalRead(BUT_RT) == LOW) buttons += 64;
        if(digitalRead(BUT_LT) == LOW) buttons += 128;
    
        Serial.write(0xff);
        Serial.write((unsigned char) right_V);
        Serial.write((unsigned char) right_H);
        Serial.write((unsigned char) left_V);
        Serial.write((unsigned char) left_H);
        Serial.write(buttons);              // buttons
        Serial.write((unsigned char) Gyro_P);
        Serial.write((unsigned char) Gyro_R);
        Serial.write((unsigned char) Gyro_Y);
        Serial.write((unsigned char)(255 - (right_V+right_H+left_V+left_H+Gyro_P+Gyro_R+Gyro_Y+buttons)%256));
    
        if(i > 12){
          digitalWrite(10,HIGH-digitalRead(10));
          i=0;
    
        }
        i++;
        delay(FRAME_LEN); 
    }



    Code:
    int Commander::ReadMsgs(){
      while(XBeeSerial.available() > 0){
        if(index == -1){         // looking for new packet
          if(XBeeSerial.read() == 0xff){
            index = 0;
            checksum = 0;
          }
        }
        else if(index == 0){
          vals[index] = (unsigned char) XBeeSerial.read();
          if(vals[index] != 0xff){            
            checksum += (int) vals[index];
            index++;
          }
        }
        else{
          vals[index] = (unsigned char) XBeeSerial.read();
          checksum += (int) vals[index];
          index++;
          if(index == 12){ // packet complete
            if(checksum%256 != 255){
              // packet error!
              index = -1;
              return 0;
            }
            else{
              digitalWrite(USER, digitalRead(USER)? LOW : HIGH);
              rightV = (signed char)( (int)vals[0]-128 );
              rightH = (signed char)( (int)vals[1]-128 );
              leftV = (signed char)( (int)vals[2]-128 );
              leftH = (signed char)( (int)vals[3]-128 );
              buttons = vals[4];    
              Gyro_P = (signed char)( (int)vals[5]-128 );
              Gyro_R = (signed char)( (int)vals[6]-128 );
              Gyro_Y = (signed char)( (int)vals[7]-128 );
    
            }
            index = -1;
            while (XBeeSerial.read() != -1)
              ;
            return 1;
          }
        }
      }
      return 0;
    }
    Last edited by KingPin; 12-10-2017 at 10:51 AM.

  2. #2

    Re: Trying to add a gyro tot arbotix commander

    Sorry, don't have time right now to fully debug, nor is there enough information (like all of the code on both sides).

    But some of the things I would look at include:

    Why is it failing? Data incorrect for the new values or bad checksum or ??? The checksum calculations might be interesting if for example the Gyro values are not properly in the 0-255 range. For example suppose one is -1... When you cast it to unsigned char to output it, it would do one thing, but when you simply add it to your checksum, might do something completely different...

    On receiving side. I assumed you updated the line:
    if(index == 12){ // packet complete

    So you output:
    ff rv rh lv lh bt gp gr gy <checksum>

    So again not sure if your check for 12 is correct. I see 10 bytes total

    Also not sure what the size of vals array is. Did you increase it's size to hole the new data?

  3. Re: Trying to add a gyro tot arbotix commander

    No pro, this is already very helpfull.

    I am looking in the checksum right now, hence my need for a way of debugging.

    I did debug the new values, they are all within the 0-255 range.

    About the update index==10 to index==12. I also counted also only 10 bytes, but in the original code I counted only 8 and in this version index==10 was used. So I just guessed that my error was there and tried index==12.

    And damn, stupid...i forgot to update the size of the vals array...

  4. Re: Trying to add a gyro tot arbotix commander

    @KurtEck got it working

    in Phoenix_input_commander:
    Code:
    unsigned char vals[9];
    and
    Code:
    if(index == 9){ // packet complete
    and in Commander don't change
    Code:
    if(i > 10){
    did the trick.

    Thanks again

Thread Information

Users Browsing this Thread

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

Similar Threads

  1. Project Pololu's Wixel + Arbotix Commander V2
    By Gertlex in forum Arbotix, Microcontrollers, Arduino
    Replies: 7
    Last Post: 07-09-2019, 09:48 PM
  2. Project Arbotix commander Oled display
    By Lynlimer in forum Project Showcase
    Replies: 3
    Last Post: 11-04-2017, 11:15 AM
  3. Question(s) Need help programming Arbotix Commander (from quad kit)
    By lumpen5 in forum Software and Programming
    Replies: 8
    Last Post: 11-09-2012, 11:57 AM
  4. Question(s) Controlling Dynamixel with Arbotix Commander
    By Buell24 in forum Software and Programming
    Replies: 9
    Last Post: 01-04-2011, 12:47 PM
  5. Question(s) Arbotix Commander
    By elaughlin in forum Robotics General Discussion
    Replies: 2
    Last Post: 10-24-2010, 12:33 AM

Tags for this Thread

Posting Permissions

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