Beispiel #1
0
void PhoenixCore::init(void)
{
    printf(F("%s\n"), __PRETTY_FUNCTION__);

    mServo->init();
    mCurVolt = mServo->getBattVolt();
    mTimerLastCheck = millis();

    //Tars init Positions
    for (u8 i = 0; i < CONFIG_NUM_LEGS; i++ ) {
        mLegPosXs[i] = (s16)pgm_read_word(&TBL_INT_POS_X[i]);    //Set start positions for each leg
        mLegPosYs[i] = (s16)pgm_read_word(&TBL_INT_POS_Y[i]);
        mLegPosZs[i] = (s16)pgm_read_word(&TBL_INT_POS_Z[i]);
    }

    initCtrl();

    //doGait
    mPtrCtrlState->bGaitType = 1;  // 0; Devon wanted
    mPtrCtrlState->sLegLiftHeight = 50;
    mGaitStep = 1;
    selectGait(mPtrCtrlState->bGaitType);

    // Servo Driver
    mCurServoMoveTime = 150;
    printf(F("up_side_down:%d\n"), mBoolUpsideDown);
}
Beispiel #2
0
void Channel::setcc(int num, int value)
      {
      cc[num] = value;

// printf("setcc %d %d\n", num, value);

      switch (num) {
            case SUSTAIN_SWITCH:
                  if (value < 64)
                        synth->damp_voices(channum);
                  break;

            case BANK_SELECT_MSB:
                  {
                  bank_msb = value & 0x7f;
                  setBanknum(bank_msb << 7);
                  }
                  break;

            case BANK_SELECT_LSB:
                  /* FIXME: according to the Downloadable Sounds II specification,
                     bit 31 should be set when we receive the message on channel
                     10 (drum channel) */
                  setBanknum((value & 0x7f) + (bank_msb << 7));
                  break;

            case ALL_NOTES_OFF:
                  synth->allNotesOff(channum);
                  break;

            case ALL_SOUND_OFF:
                  synth->allSoundsOff(channum);
                  break;

            case ALL_CTRL_OFF:
                  initCtrl();
                  synth->modulate_voices_all(channum);
                  break;

            case DATA_ENTRY_MSB:
                  {
                  int data = (value << 7) + cc[DATA_ENTRY_LSB];

                  /* SontFont 2.01 NRPN Message (Sect. 9.6, p. 74)  */
                  if ((cc[NRPN_MSB] == 120) && (cc[NRPN_LSB] < 100)) {
                        float val = fluid_gen_scale_nrpn(nrpn_select, data);
                        qDebug("%s: %d: Data = %d, value = %f", __FILE__, __LINE__, data, val);
                        synth->set_gen(channum, nrpn_select, val);
                        }
                  break;
                  }

            case NRPN_MSB:
                  cc[NRPN_LSB] = 0;
                  nrpn_select = 0;
                  break;

            case NRPN_LSB:
                  /* SontFont 2.01 NRPN Message (Sect. 9.6, p. 74)  */
                  if (cc[NRPN_MSB] == 120) {
                        if (value == 100)
                              nrpn_select += 100;
                        else if (value == 101)
                              nrpn_select += 1000;
                        else if (value == 102)
                              nrpn_select += 10000;
                        else if (value < 100) {
                              nrpn_select += value;
                              qDebug("%s: %d: NRPN Select = %d", __FILE__, __LINE__, nrpn_select);
                              }
                        }
                  break;

            case RPN_MSB:
                  break;

            case RPN_LSB:
                  /* erase any previously received NRPN message  */
                  cc[NRPN_MSB] = 0;
                  cc[NRPN_LSB] = 0;
                  nrpn_select  = 0;
                  break;

            default:
                  synth->modulate_voices(channum, true, num);
            }
      }
Beispiel #3
0
void Channel::reset()
      {
      init();
      initCtrl();
      }
/**
 * The main function of BaseStation for Mongol.
 *
 * Entry point of program. Upon starting with normal mode values set,
 * this function parses any command line arguments, initializes the controller
 * interface (\sa Controller.h), intitializes the robot communication interface
 * (\sa Serial.h), and then enters the main control loop of the program. There,
 * the program basically gathers user input data from the controller, builds a
 * serial packet, and then sends it to the robot. The program waits for
 * confirmation from the robot in the form of a PKT_RDY packet type before
 * looping again.
 */
int
main (int argc, char* argv[])
{
    // Set default values
    m_ctrl_mode = Ctrl_Mode_Keyboard;
    m_comm_mode = Comm_Mode_Offline;
    m_ui_mode   = UI_Mode_Terminal;

    // Parse command line arguements
    parse_args (argc, argv);

    // Initialize SDL (VIDEO flag also initializes event handling)
    print_log ("Initializing controller... ", Log_Level_Med);
    if (!(initCtrl()))
    {
        print_log ("Controller failed to intialize.\n", Log_Level_High);
        quit_basestation ();
    }
    print_log ("Controller Initialized.\n", Log_Level_Med);

    // Initialize serial port (includes looking for HELLO packet
    // If not port name specified, default to /dev/ttyUSB0 (for Linux)
    if (m_comm_mode == Comm_Mode_Online)
    {
        print_log ("Connecting to robot... ", Log_Level_Med);
        if ((ms_comm_device ? init_serial (ms_comm_device) : init_serial ("/dev/ttyUSB0")) < 0)
        {
            print_log ("Failed to connect\n", Log_Level_High);
            m_comm_mode = Comm_Mode_Offline;
            quit_basestation ();
        }
        print_log ("Robot connected.\n", Log_Level_Med);
    }

    Msg out_msg;
    Msg in_msg;
    char intype_buf [128];
    char outtype_buf [128];
    int print_nxt_received = 0;

    // Main program loop
    for(;;)
    {
        next_event (&out_msg);

        if(m_comm_mode == Comm_Mode_Online)
        {
            do
            {
                serial_write (&out_msg);
                usleep (10);
            }
            while (serial_read (&in_msg) ||
                   (in_msg.type != Msg_Type_Ready));

            if (out_msg.type != Msg_Type_Standby)
            {
                type_to_str (intype_buf, out_msg.type);
                printf ("Sent: %s\n", intype_buf);
                type_to_str (outtype_buf, in_msg.type);
                printf ("Received: %s\n", outtype_buf);
            }
        }
    }

    return 0;
}