init.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "TheStrut.h"
#include "../Globals.h"

// duh!
// values obtained from sony documentation !
void TheStrut::SetJointGain() {
  if (!motorPowerOn_) return;
#ifdef ERS_7
  OPENR::EnableJointGain(headJointID[TILT1_INDEX]);
  OPENR::SetJointGain(headJointID[TILT1_INDEX],TILT1_PGAIN,TILT1_IGAIN,TILT1_DGAIN,PSHIFT, ISHIFT, DSHIFT);
  OPENR::EnableJointGain(headJointID[PAN_INDEX]);
  OPENR::SetJointGain(headJointID[PAN_INDEX],PAN_PGAIN,PAN_IGAIN,PAN_DGAIN,PSHIFT, ISHIFT, DSHIFT);
  OPENR::EnableJointGain(headJointID[TILT2_INDEX]);
  OPENR::SetJointGain(headJointID[TILT2_INDEX],TILT2_PGAIN,TILT2_IGAIN,TILT2_DGAIN,PSHIFT, ISHIFT, DSHIFT);
#endif
#ifdef ERS_210
  OPENR::EnableJointGain(headJointID[TILT_INDEX]);
  OPENR::SetJointGain(headJointID[TILT_INDEX],TILT_PGAIN,TILT_IGAIN,TILT_DGAIN,PSHIFT, ISHIFT, DSHIFT);
  OPENR::EnableJointGain(headJointID[PAN_INDEX]);
  OPENR::SetJointGain(headJointID[PAN_INDEX],PAN_PGAIN,PAN_IGAIN,PAN_DGAIN,PSHIFT, ISHIFT, DSHIFT);
  OPENR::EnableJointGain(headJointID[ROLL_INDEX]);
  OPENR::SetJointGain(headJointID[ROLL_INDEX],ROLL_PGAIN,ROLL_IGAIN,ROLL_DGAIN,PSHIFT, ISHIFT, DSHIFT);
#endif

  OPENR::EnableJointGain(otherJointID[MOUTH_INDEX]);
  OPENR::SetJointGain(otherJointID[MOUTH_INDEX], MOUTH_PGAIN, MOUTH_IGAIN, MOUTH_DGAIN, PSHIFT, ISHIFT, DSHIFT);

/*
  OPENR::EnableJointGain(otherJointID[L_EAR_INDEX]);
  OPENR::SetJointGain(otherJointID[L_EAR_INDEX], L_EAR_PGAIN, L_EAR_IGAIN, L_EAR_DGAIN, PSHIFT, ISHIFT, DSHIFT);
  OPENR::EnableJointGain(otherJointID[R_EAR_INDEX]);
  OPENR::SetJointGain(otherJointID[R_EAR_INDEX], R_EAR_PGAIN, R_EAR_IGAIN, R_EAR_DGAIN, PSHIFT, ISHIFT, DSHIFT);
*/
#ifdef TAIL_ENABLED
  OPENR::EnableJointGain(otherJointID[TAIL_HORI_INDEX]);
  OPENR::SetJointGain(otherJointID[TAIL_HORI_INDEX], TAIL_HORI_PGAIN, TAIL_HORI_IGAIN, TAIL_HORI_DGAIN, PSHIFT, ISHIFT, DSHIFT);
  OPENR::EnableJointGain(otherJointID[TAIL_VERT_INDEX]);
  OPENR::SetJointGain(otherJointID[TAIL_VERT_INDEX], TAIL_VERT_PGAIN, TAIL_VERT_IGAIN, TAIL_VERT_DGAIN, PSHIFT, ISHIFT, DSHIFT);
#endif

  for (unsigned int i = 0; i < 4; i++) {
    int j1 = 3 * i;
    int j2 = 3 * i + 1;
    int j3 = 3 * i + 2; 
    OPENR::EnableJointGain(bodyJointID[j1]);        
    OPENR::SetJointGain(bodyJointID[j1],J1_PGAIN, J1_IGAIN, J1_DGAIN,PSHIFT, ISHIFT, DSHIFT);
    OPENR::EnableJointGain(bodyJointID[j2]);        
    OPENR::SetJointGain(bodyJointID[j2],J2_PGAIN, J2_IGAIN, J2_DGAIN,PSHIFT, ISHIFT, DSHIFT);
    OPENR::EnableJointGain(bodyJointID[j3]);        
    OPENR::SetJointGain(bodyJointID[j3],J3_PGAIN, J3_IGAIN, J3_DGAIN,PSHIFT, ISHIFT, DSHIFT);
  }
}

void TheStrut::SetJointGainManual() {
  if (!motorPowerOn_) return;
  if (configuration_.GetAsBool("ManualJointGain") == true) {
    cout << "WARNING! Manual joint gain control is enabled." << endl << flush;
    for (unsigned int i = 0; i < 4; i++) {
      int j1 = 3 * i;
      int j2 = 3 * i + 1;
      int j3 = 3 * i + 2; 
      OPENR::EnableJointGain(bodyJointID[j1]);        
      OPENR::SetJointGain(bodyJointID[j1],configuration_.GetAsInt("J1_PGAIN"), configuration_.GetAsInt("J1_IGAIN"), configuration_.GetAsInt("J1_DGAIN"),PSHIFT, ISHIFT, DSHIFT);
      OPENR::EnableJointGain(bodyJointID[j2]);        
      OPENR::SetJointGain(bodyJointID[j2],configuration_.GetAsInt("J2_PGAIN"), configuration_.GetAsInt("J2_IGAIN"), configuration_.GetAsInt("J2_DGAIN"),PSHIFT, ISHIFT, DSHIFT);
      OPENR::EnableJointGain(bodyJointID[j3]);        
      OPENR::SetJointGain(bodyJointID[j3],configuration_.GetAsInt("J3_PGAIN"), configuration_.GetAsInt("J3_IGAIN"), configuration_.GetAsInt("J3_DGAIN"),PSHIFT, ISHIFT, DSHIFT);
    }
  }
}


// set up some stuff for joint communication
void TheStrut::OpenPrimitives() {
  for (unsigned int i = 0; i < NUM_BODY_JOINTS; i++) {
    OPENR::OpenPrimitive(BODY_JOINT[i], &bodyJointID[i]);
  }
  for (unsigned int i = 0; i < NUM_HEAD_JOINTS; i++) {
    OPENR::OpenPrimitive(HEAD_JOINT[i], &headJointID[i]);
  }
  for (unsigned int i = 0; i < NUM_OTHER_JOINTS; i++) {
    OPENR::OpenPrimitive(OTHER_JOINT[i], &otherJointID[i]);
  }
  for (unsigned int i = 0; i < NUM_LEDS+NUM_LEDS_2; i++) {
    OPENR::OpenPrimitive(LED_PRIMITIVES[i], &ledID[i]);
  }
}

// send first command to joints (just sets them to their current positions)
void TheStrut::InitialiseJoints() {
  for (unsigned int j = 0; j < NUM_BUFFERS; j++) {
    for (unsigned int i = 0; i < NUM_BODY_JOINTS; i++) {
      OCommandInfo* info = bodyVec[j]->GetInfo(i);
      info->Set(odataJOINT_COMMAND2, bodyJointID[i], ocommandMAX_FRAMES);

      OCommandData* jointData = bodyVec[j]->GetData(i);
      bodyValue[i] = (OJointCommandValue2*)jointData->value;

      OJointValue current;
      OPENR::GetJointValue(bodyJointID[i], &current);
      for (unsigned int k = 0; k < ocommandMAX_FRAMES; k++) {
        bodyValue[i][k].value = current.value;

        //SetBodyJoint(&bodyValue[i][k].value, current.value, 0);
      }
    }
    for (unsigned int i = 0; i < NUM_HEAD_JOINTS; i++) {
      OCommandInfo* info = headVec[j]->GetInfo(i);
      info->Set(odataJOINT_COMMAND2, headJointID[i], ocommandMAX_FRAMES);
      OCommandData* jointData = headVec[j]->GetData(i);
      headValue[i] = (OJointCommandValue2*)jointData->value;

      OJointValue current;
      OPENR::GetJointValue(headJointID[i], &current);
      for (unsigned int k = 0; k < ocommandMAX_FRAMES; k++) {
        headValue[i][k].value = current.value;
        //SetHeadJoint(&headValue[i][k].value, current.value, 0);
      }
    }
    for (unsigned int i = 0; i < NUM_OTHER_JOINTS; i++) {
      OCommandInfo* info = otherVec[j]->GetInfo(i);
      info->Set(odataJOINT_COMMAND2, otherJointID[i], ocommandMAX_FRAMES);
    
      OCommandData* jointData = otherVec[j]->GetData(i);
      otherValue[i] = (OJointCommandValue2*)jointData->value;

      OJointValue current;
      OPENR::GetJointValue(otherJointID[i], &current);
      for (unsigned int k = 0; k < ocommandMAX_FRAMES; k++) {
        otherValue[i][k].value = current.value;
      }
    }
    for (unsigned int i = 0; i < NUM_LEDS; i++) {
      OCommandInfo* info = ledVec[j]->GetInfo(i);
      info->Set(odataLED_COMMAND2, ledID[i], 1);

      OCommandData* ledData = ledVec[j]->GetData(i);
      ledValue[i] = (OLEDCommandValue2*)ledData->value;
    }
#ifdef ERS_7
    for (unsigned int i = 0; i < NUM_LEDS_2; i++) {
      int index = i + NUM_LEDS;
      OCommandInfo* info = ledVec[j]->GetInfo(index);
      info->Set(odataLED_COMMAND3, ledID[index], 1);

      OCommandData* ledData = ledVec[j]->GetData(index);
      ledValue2[i] = (OLEDCommandValue3*)ledData->value;
    }
#endif
  }

  sbjCommandVector->SetData(cmdBodyRegion[0]);
  sbjCommandVector->SetData(cmdOtherRegion[0]);
  sbjCommandVector->SetData(cmdHeadRegion[0]);
  jointsInitialised = true;
}


// set up memory structures for sending stuff to joints
void TheStrut::SetupBuffers() {
  for (unsigned int j = 0; j < NUM_BUFFERS; j++) {
    OPENR::NewCommandVectorData(NUM_HEAD_JOINTS, &headVecID[j], &headVec[j]);
    OPENR::NewCommandVectorData(NUM_BODY_JOINTS, &bodyVecID[j], &bodyVec[j]);
    OPENR::NewCommandVectorData(NUM_OTHER_JOINTS, &otherVecID[j], &otherVec[j]);
    OPENR::NewCommandVectorData(NUM_LEDS+NUM_LEDS_2, &ledVecID[j], &ledVec[j]);
 
    headVec[j]->SetNumData(NUM_HEAD_JOINTS);
    bodyVec[j]->SetNumData(NUM_BODY_JOINTS);
    otherVec[j]->SetNumData(NUM_OTHER_JOINTS);
    ledVec[j]->SetNumData(NUM_LEDS+NUM_LEDS_2);

    cmdHeadRegion[j] = new RCRegion(headVec[j]->vectorInfo.memRegionID, headVec[j]->vectorInfo.offset, (void*)headVec[j], headVec[j]->vectorInfo.totalSize);
    cmdBodyRegion[j] = new RCRegion(bodyVec[j]->vectorInfo.memRegionID, bodyVec[j]->vectorInfo.offset, (void*)bodyVec[j], bodyVec[j]->vectorInfo.totalSize);
    cmdOtherRegion[j] = new RCRegion(otherVec[j]->vectorInfo.memRegionID, otherVec[j]->vectorInfo.offset, (void*)otherVec[j], otherVec[j]->vectorInfo.totalSize);
    cmdLedRegion[j] = new RCRegion(ledVec[j]->vectorInfo.memRegionID, ledVec[j]->vectorInfo.offset, (void*)ledVec[j], ledVec[j]->vectorInfo.totalSize);
  }
}