void WG021::packCommand(unsigned char *buffer, bool halt, bool reset)
{
pr2_hardware_interface::ProjectorCommand &cmd = projector_.command_;
// Override enable if motors are halted
if (reset)
{
clearErrorFlags();
}
resetting_ = reset;
// Truncate the current to limit (do not allow negative current)
projector_.state_.last_commanded_current_ = cmd.current_;
cmd.current_ = max(min(cmd.current_, max_current_), 0.0);
// Pack command structures into EtherCAT buffer
WG021Command *c = (WG021Command *)buffer;
memset(c, 0, command_size_);
c->digital_out_ = digital_out_.command_.data_;
c->programmed_current_ = int(cmd.current_ / config_info_.nominal_current_scale_);
c->mode_ = (cmd.enable_ && !halt && !has_error_) ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
c->mode_ |= reset ? MODE_SAFETY_RESET : 0;
c->config0_ = ((cmd.A_ & 0xf) << 4) | ((cmd.B_ & 0xf) << 0);
c->config1_ = ((cmd.I_ & 0xf) << 4) | ((cmd.M_ & 0xf) << 0);
c->config2_ = ((cmd.L1_ & 0xf) << 4) | ((cmd.L0_ & 0xf) << 0);
c->general_config_ = cmd.pulse_replicator_ == true;
c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
}
void BaseAudioFile::close()
{
setFileType(kAudioFileNone);
BaseAudioFile::setPCMFormat(kAudioFileInt8);
setHeaderEndianness(kAudioFileLittleEndian);
setAudioEndianness(kAudioFileLittleEndian);
setSamplingRate(0);
setChannels(0);
setFrames(0);
setPCMOffset(0);
clearErrorFlags();
}
void receiveByte(uint8_t adr, uint8_t subAdr, uint8_t *buffer)
{
clearErrorFlags();
I2C001_DataType data1;
data1.Data1.TDF_Type = I2C_TDF_MStart;
data1.Data1.Data = ((adr << 1) | I2C_WRITE);
while(!I2C001_WriteData(&I2C001_Handle0,&data1))
{
flushFIFO();
}
delay(DELAY);
I2C001_DataType data2;
data2.Data1.TDF_Type = I2C_TDF_MTxData;
data2.Data1.Data = subAdr;
while(!I2C001_WriteData(&I2C001_Handle0,&data2))
{
flushFIFO();
}
delay(DELAY);
I2C001_DataType data3;
data3.Data1.TDF_Type = I2C_TDF_MRStart;
//uint8_t adr1 = address->adr.addressDevice;
data3.Data1.Data = ((adr << 1) | I2C_READ);
while(!I2C001_WriteData(&I2C001_Handle0,&data3))
{
flushFIFO();
}
delay(DELAY);
I2C001_DataType data4;
data4.Data1.TDF_Type = I2C_TDF_MRxAck1;
data4.Data1.Data = ubyteFF;
while(!I2C001_WriteData(&I2C001_Handle0,&data4))
{
flushFIFO();
}
delay(DELAY);
I2C001_DataType data5;
data5.Data1.TDF_Type = I2C_TDF_MStop;
data5.Data1.Data = ubyteFF;
while(!I2C001_WriteData(&I2C001_Handle0,&data5))
{
flushFIFO();
}
delay(DELAY);
int k = 0;
uint16_t bufferToRead = 0;
if(I2C001_ReadData(&I2C001_Handle0,&bufferToRead))
{
k++;
}
else
{
k--;
}
delay(DELAY);
*buffer = (uint8_t)bufferToRead;
}
