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