/* InitToPose - gently moves Darwin into a ready position with speed 0x40
* Return - true if successful
* - false if unable to set torque, or if write fails
*/
bool DarwinController::InitToPose(){
unsigned char initpacket[108] = {0, };
unsigned char initparams[100] = {0, };
// Red eyes - Initialization in progress
int color = MakeColor(255, 0, 0);
unsigned char colorparams[2] = {GetLowByte(color), GetHighByte(color)};
MakePacket(initpacket, 0xC8, 2, 0x03, 0x1C, colorparams);
port.ClearPort();
port.WritePort(initpacket, 9);
// Torque enable
for(int IDnum = 0; IDnum < 21; IDnum++){
initparams[2*IDnum] = (unsigned char)(IDnum+1); // +1 because motors start at 1
initparams[2*IDnum+1] = 0x01;
}
unsigned char paramlength = 1;
unsigned char initnumparams = 40;
if(SyncWrite(initpacket, 0x18, initparams, initnumparams, paramlength) != 48){
printf("Failed Torque Enable in InitToPose!\n");
return false;
}
usleep(2000);
// Starting position and speed
for(int z = 0; z < 20; z++){
initparams[5*z] = z+1;
initparams[5*z+1] = 0x00;
initparams[5*z+2] = 0x08; // "zero" neutral position is 2048
initparams[5*z+3] = 0x40; // speed is 0x40
initparams[5*z+4] = 0x00;
}
if(SyncWrite(initpacket, 0x1E, initparams, 100, 4) != 108){
printf("Failed to init to pose\n");
return false;
}
sleep(1);
// Green eyes - Finished initializing
color = MakeColor(0, 255, 0);
initpacket[6] = GetLowByte(color);
initpacket[7] = GetHighByte(color);
colorparams[0] = GetLowByte(color);
colorparams[1] = GetHighByte(color);
MakePacket(initpacket, 0xC8, 2, 0x03, 0x1C, colorparams);
port.ClearPort();
port.WritePort(initpacket, 9);
return true;
}
void APU_INTERNAL::Reset( FLOAT fClock, INT nRate )
{
ZEROMEMORY( &ch0, sizeof(ch0) );
ZEROMEMORY( &ch1, sizeof(ch1) );
ZEROMEMORY( &ch2, sizeof(ch2) );
ZEROMEMORY( &ch3, sizeof(ch3) );
// ZEROMEMORY( &ch4, sizeof(ch4) );
ZEROMEMORY( bToneTableEnable, sizeof(bToneTableEnable) );
ZEROMEMORY( ToneTable, sizeof(ToneTable) );
ZEROMEMORY( ChannelTone, sizeof(ChannelTone) );
reg4015 = sync_reg4015 = 0;
// Sweep complement
ch0.complement = 0x00;
ch1.complement = 0xFF;
// Noise shift register
ch3.shift_reg = 0x4000;
Setup( fClock, nRate );
// $4011は初期化しない
WORD addr;
for( addr = 0x4000; addr <= 0x4010; addr++ ) {
Write( addr, 0x00 );
SyncWrite( addr, 0x00 );
}
// Write( 0x4001, 0x08 ); // Reset時はincモードになる?
// Write( 0x4005, 0x08 ); // Reset時はincモードになる?
Write( 0x4012, 0x00 );
Write( 0x4013, 0x00 );
Write( 0x4015, 0x00 );
SyncWrite( 0x4012, 0x00 );
SyncWrite( 0x4013, 0x00 );
SyncWrite( 0x4015, 0x00 );
// $4017は書き込みで初期化しない(初期モードが0であるのを期待したソフトがある為)
FrameIRQ = 0xC0;
FrameCycle = 0;
FrameIRQoccur = 0;
FrameCount = 0;
FrameType = 0;
// ToneLoad
ToneTableLoad();
}
static void Reset( APU_INTERNAL *pme, FLOAT fClock, INT nRate )
{
WORD addr;
ZEROMEMORY( &pme->ch0, sizeof(pme->ch0) );
ZEROMEMORY( &pme->ch1, sizeof(pme->ch1) );
ZEROMEMORY( &pme->ch2, sizeof(pme->ch2) );
ZEROMEMORY( &pme->ch3, sizeof(pme->ch3) );
// ZEROMEMORY( &pme->ch4, sizeof(pme->ch4) );
ZEROMEMORY( pme->bToneTableEnable, sizeof(pme->bToneTableEnable) );
ZEROMEMORY( pme->ToneTable, sizeof(pme->ToneTable) );
ZEROMEMORY( pme->ChannelTone, sizeof(pme->ChannelTone) );
pme->reg4015 = pme->sync_reg4015 = 0;
// Sweep complement
pme->ch0.complement = 0x00;
pme->ch1.complement = 0xFF;
// Noise shift register
pme->ch3.shift_reg = 0x4000;
Setup( pme, fClock, nRate );
// $4011は初期化しない
for( addr = 0x4000; addr <= 0x4010; addr++ ) {
Write( pme, addr, 0x00 );
SyncWrite( pme, addr, 0x00 );
}
// Write( pme, 0x4001, 0x08 ); // Reset時はincモードになる?
// Write( pme, 0x4005, 0x08 ); // Reset時はincモードになる?
Write( pme, 0x4012, 0x00 );
Write( pme, 0x4013, 0x00 );
Write( pme, 0x4015, 0x00 );
SyncWrite( pme, 0x4012, 0x00 );
SyncWrite( pme, 0x4013, 0x00 );
SyncWrite( pme, 0x4015, 0x00 );
// $4017は書き込みで初期化しない(初期モードが0であるのを期待したソフトがある為)
pme->FrameIRQ = 0xC0;
pme->FrameCycle = 0;
pme->FrameIRQoccur = 0;
pme->FrameCount = 0;
pme->FrameType = 0;
// ToneLoad
ToneTableLoad(pme);
}
void XMLClient::HandleConnect(const boost::system::error_code& error) {
boost::mutex::scoped_lock lock(m);
_bWaitConnect = false;
_bConnected = true;
// The async_connect() function automatically opens the socket at the start
// of the asynchronous operation. If the socket is closed at this time then
// the timeout handler must have run first.
if (!_socket->is_open()) {
Disconnect();
return;
}
if (error == boost::asio::error::connection_refused) {
Globals::ErrorMessage("HandleConnect connect refused\n");
Globals::Sleep(_nMSServerConnectRetry);
Connect();
return;
} else if (error == boost::asio::error::already_started) {
Globals::ErrorMessage("HandleConnect connect already_started\n");
Globals::Sleep(_nMSServerConnectRetry);
Connect();
return;
}
// On error, return early.
if (error) {
std::stringstream s;
s << "HandleConnect unknown error = (" << error << ") " << error.message() << std::endl;
Globals::ErrorMessage(s.str());
return;
}
StartAyncRead();
SyncWrite(CrclInterface().CRCLInitCanonCmd());
}
void XMLClient::StartAyncRead() {
try {
Globals::ErrorMessage("StartAyncRead\n");
if (!_socket->is_open()) {
raise(SIGTRAP);
}
TimerReset();
boost::asio::async_read(*_socket,
boost::asio::buffer(data_, max_length), // buff, //boost::asio::buffer(&readBuffer[0], readBuffer.size()),
boost::asio::transfer_at_least(1),
boost::bind(&XMLClient::bytesToRead,
this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
std::string statcmd = CrclInterface().CRCLGetStatusCmd();
SyncWrite(statcmd);
Globals::Sleep(1000);
} catch (boost::exception & ex) {
//http://theboostcpplibraries.com/boost.exception
std::cerr << boost::diagnostic_information(ex);
_socket->close();
} catch (...) {
_socket->close();
}
}
bool DarwinController::SetAllJointSpeeds(unsigned char highbyte, unsigned char lowbyte){
unsigned char speedTxPacket[MAXNUM_TXPARAM];
unsigned char speedParams[60]; // 3 params each for 20 motors
for(int i = 0; i < NUM_JOINTS; i++){
speedParams[3*i] = i+1;
speedParams[3*i+1] = lowbyte;
speedParams[3*i+2] = highbyte;
}
int result = SyncWrite(speedTxPacket, 0x20, speedParams, 60, 2);
if(result == 68){
return true;
} else {
return false;
}
}
void XMLClient::SyncConnect() {
tcp::resolver resolver(*_io_service);
tcp::resolver::query query(_ipv4.c_str(), _port.c_str());
tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
tcp::resolver::iterator end;
boost::system::error_code error = boost::asio::error::host_not_found;
while (error && endpoint_iterator != end) {
//_socket->close();
_socket->connect(*endpoint_iterator++, error);
}
if (error)
return; // throw boost::system::system_error(error);
//TimerReset();
// _timer = deadline_timer_ptr(new boost::asio::deadline_timer(*_io_service,
// boost::posix_time::milliseconds(2000)));
// _timer->async_wait(boost::bind(&XMLClient::wait_callback, this, _1, boost::ref(*_socket)));
SyncWrite(CrclInterface().CRCLInitCanonCmd());
Globals::Sleep(1000);
StartAyncRead();
//boost::mutex::scoped_lock lock(m);
_bConnected = true;
#if 0
tcp::resolver resolver(_io_service);
tcp::resolver::query query(_ipv4.c_str(), _port.c_str());
tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
//boost::asio::connect(_socket, endpoint_iterator);
_bWaitConnect = true;
_socket.async_connect(*endpoint_iterator, boost::bind(&XMLClient::HandleConnect,
this,
boost::asio::placeholders::error));
#endif
// Start the deadline actor. You will note that we're not setting any
// particular deadline here. Instead, the connect and input actors will
// update the deadline prior to each asynchronous operation.
//deadline_.async_wait(boost::bind(&client::check_deadline, this));
}
// if just goal changed, write 2 bytes
// if just gains chamnged write 3 bytes
// if both changed write all 5 bytes
void foo() {
// declare an array of joint data
JointData joints[NUM_JOINTS];
// motor 3 is enabled, has new goal position
joints[3].goal = 1027;
joints[3].flags = FLAG_ENABLE | FLAG_GOAL_CHANGED;
// later:
int packet_count = 0;
uint8_t change_flags = 0;
// figure out how many packets and what data gets sent
for (int i=0; i<NUM_JOINTS; ++i) {
const JointData& ji = joints[i];
bool enabled = ji.flags & FLAG_ENABLE; // pick off top bit to see if joint enabled
uint8_t changed = ji.flags & ~FLAG_ENABLE; // pick off bottom 7 bits
if (enabled && changed) { // if this motor is enabled and has new data
change_flags |= changed; // add in changes from this motor to set of all changes
packet_count += 1; // increment number of packets to send
}
}
// at this point can tell how many bytes per motor to send
uint8_t buf[MAX_PACKET_LENGTH*MAX_NUM_PACKETS + HEADER_LEN + FOOTER_LEN];
int buf_offset = 0;
unsigned char numparams = 0;
unsigned char lenparam;
unsigned char txpacket[128] = {0, };
// todo: fill in header
if (change_flags == FLAG_GOAL_CHANGED){ // If any of Goals changed but none of the PIDs changed
lenparam = 2;
for (int i=0; i<NUM_JOINTS; ++i) {
JointData& ji = joints[i];
if (ji.flags == FLAG_ENABLE + FLAG_GOAL_CHANGED){ // If this joint's Goal has changed, add to sync write
buf[buf_offset++] = i;
buf[buf_offset++] = GetLowByte(goal);
buf[buf_offset++] = GetHighByte(goal);
ji.flags = FLAG_ENABLE; // cleared the changed bits cause we are about to send this
numparams += 3;
}
}
}
else if (change_flags == FLAG_GAINS_CHANGED){ // If any of the gains changed but none of the Goals changed
lenparam = 3;
for (int i=0; i<NUM_JOINTS; ++i) {
JointData& ji = joints[i];
if (ji.flags == FLAG_ENABLE + FLAG_GAINS_CHANGED){ // If this joint's gains have changed, add to sync write
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
ji.flags = FLAG_ENABLE; // cleared the changed bits cause we are about to send this
numparams += 4;
}
}
}
else if (change_flags == FLAG_GAINS_CHANGED + FLAG_GOAL_CHANGED){ // If both PID and Goal have changed
lenparam = 5;
for (int i=0; i<NUM_JOINTS; ++i) {
JointData& ji = joints[i];
if (ji.flags & FLAG_GOAL_CHANGED || ji.flags & FLAG_GAINS_CHANGED && ji.flags & FLAG_ENABLE){ // If this joint's Goal has changed, add to sync write
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
buf[buf_offset++] = GetLowByte(goal);
buf[buf_offset++] = GetHighByte(goal);
ji.flags = FLAG_ENABLE; // cleared the changed bits cause we are about to send this
numparams += 6;
}
}
}
SyncWrite(txpacket, 0x03, buf, numparams, lenparam);
port->ClearPort();
port->WritePort(txpacket, numparams + 8);
}
// call this to write the changes in the JointData struct out to port
void DarwinController::Update_Motors(){
int packet_count = 0;
uint8_t change_flags = 0;
// figure out how many packets and what data gets sent
for (int i=1; i<NUM_JOINTS+1; ++i) {
const JointData& ji = joints[i];
// pick off top bit to see if joint enabled
bool enabled = ji.flags & FLAG_ENABLE;
// pick off bottom 7 bits
uint8_t changed = ji.flags & ~FLAG_ENABLE;
// if this motor is enabled and has new data
if (enabled && changed) {
// add in changes from this motor to set of all changes
change_flags |= changed;
// increment number of packets to send
packet_count += 1;
}
}
// at this point can tell how many bytes per motor to send
uint8_t buf[120] = {0, };
int buf_offset = 0;
unsigned char numparams = 0;
unsigned char lenparam;
unsigned char txpacket[128] = {0, };
unsigned char inst;
// If any of Goals changed but none of the PIDs changed
if (change_flags == FLAG_GOAL_CHANGED){
lenparam = 2;
inst = 0x1E; //starting address for goal position Low
for (int i=0; i<NUM_JOINTS+1; ++i) {
JointData& ji = joints[i];
// If this joint's Goal has changed, add to sync write
if (ji.flags == FLAG_ENABLE + FLAG_GOAL_CHANGED){
buf[buf_offset++] = i;
buf[buf_offset++] = GetLowByte(ji.goal);
buf[buf_offset++] = GetHighByte(ji.goal);
// cleared the changed bits cause we are about to send this
ji.flags = FLAG_ENABLE;
numparams += 3;
}
}
}
// If any of the gains changed but none of the Goals changed
else if (change_flags == FLAG_GAINS_CHANGED){
lenparam = 3;
inst = 0x1A; // starting address for pid gains
for (int i=0; i<NUM_JOINTS+1; ++i) {
JointData& ji = joints[i];
// If this joint's gains have changed, add to sync write
if (ji.flags == FLAG_ENABLE + FLAG_GAINS_CHANGED){
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
// cleared the changed bits cause we are about to send this
ji.flags = FLAG_ENABLE;
numparams += 4;
}
}
}
// If both PID and Goal have changed
else if (change_flags == FLAG_GAINS_CHANGED + FLAG_GOAL_CHANGED){
lenparam = 6;
inst = 0x1A; //starting address for pid gains
for (int i=0; i<NUM_JOINTS+1; ++i) {
JointData& ji = joints[i];
// If this joint's Goal has changed, add to sync write
if (ji.flags & FLAG_GOAL_CHANGED || ji.flags & FLAG_GAINS_CHANGED && ji.flags & FLAG_ENABLE){
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
buf[buf_offset++] = 0x00; // I hate everything
buf[buf_offset++] = GetLowByte(ji.goal); // shhh it's ok
buf[buf_offset++] = GetHighByte(ji.goal);
// cleared the changed bits cause we are about to send this
ji.flags = FLAG_ENABLE;
numparams += 7;
}
}
}
if(packet_count){
SyncWrite(txpacket, inst, buf, numparams, lenparam);
}
}
