Trying to add a gyro tot arbotix commander

[KingPin]

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?

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); 
}

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;
}

[KurtEck]

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?

[KingPin]

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...

[KingPin]

@KurtEck got it working

in Phoenix_input_commander:

unsigned char vals[9];

and

if(index == 9){ // packet complete

and in Commander don't change

if(i > 10){

did the trick.

Thanks again