OutputInfo FullyConnected::initialize(std::vector<double*>& parameterPointers,
std::vector<double*>& parameterDerivativePointers)
{
parameterPointers.reserve(parameterPointers.size() + J * (I + bias));
parameterDerivativePointers.reserve(parameterDerivativePointers.size() + J * (I + bias));
for(int j = 0; j < J; j++)
{
for(int i = 0; i < I; i++)
{
parameterPointers.push_back(&W(j, i));
parameterDerivativePointers.push_back(&Wd(j, i));
}
if(bias)
{
parameterPointers.push_back(&b(j));
parameterDerivativePointers.push_back(&bd(j));
}
}
initializeParameters();
OutputInfo info;
info.dimensions.push_back(J);
return info;
}
void ConnectUsbtinyPort(struct UPort *up) {
Assert(up->port.kind == 'u');
// Ws(" -- ConnectUsbtinyPort entry. "); WriteUPort(up);
Ws("UsbTiny"); Wd(up->port.index,1); Wc(' ');
jmp_buf oldFailPoint;
memcpy(oldFailPoint, FailPoint, sizeof(FailPoint));
if (setjmp(FailPoint)) {
// Wsl(" -- Fail caught in ConnectUsbtinyPort.");
up->port.baud = -1;
} else {
up->port.baud = 0;
if (!up->handle) {up->handle = usb_open(up->device);}
if (!up->handle) {PortFail(up, "Couldn't open UsbTiny port.");}
DigisparkBreakAndSync(up);
}
memcpy(FailPoint, oldFailPoint, sizeof(FailPoint));
Ws("\r \r");
// Ws(" -- ConnectUsbtinyPort complete. "); WriteUPort(up);
}
void DescribePort(int i) {
Assert(i < PortCount);
if (Ports[i]->character < 0) {
Ws("Unknown device ");
} else {
Ws(Characteristics[Ports[i]->character].name);
}
Ws(" on ");
if (Ports[i]->kind == 's') {
#if windows
Ws("COM");
#elif defined(__APPLE__)
Ws("/dev/tty.usbserial");
#else
Ws("/dev/ttyUSB");
#endif
} else {
Ws("UsbTiny");
}
Wd(Ports[i]->index,1); Ws(" at ");
Wd(Ports[i]->baud,1);
Wsl(" baud.");
}
void digisparkUSBSendBytes(struct UPort *up, u8 state, char *out, int outlen) {
int tries = 0;
int status = usb_control_msg(up->handle, OUT_TO_LW, 60, state, 0, out, outlen, USB_TIMEOUT);
while ((tries < 200) && (status <= 0)) {
// Wait for previous operation to complete
tries++;
delay(5);
status = usb_control_msg(up->handle, OUT_TO_LW, 60, state, 0, out, outlen, USB_TIMEOUT);
}
if (status < outlen) {Ws("Failed to send bytes to AVR, status "); Wd(status,1); PortFail(up, "");}
delay(3); // Wait at least until digispark starts to send the data.
}
void DumpConfig(void) {
u8 cb;
u8 hfuse;
u8 efuse;
// Show Known device info
Ws(Name()); Wsl(".");
Ws("IO regs: "); Wx(32,2); Ws(".."); Wx(IoregSize()+31,2); Ws(" ("); Wd(IoregSize(),1); Wsl(" bytes).");
Ws("SRAM: "); Wx(IoregSize()+32,3); Ws(".."); Wx(31 + IoregSize() + SramSize(), 3); Ws(" ("); Wd(SramSize(),1); Wsl(" bytes).");
Ws("Flash: 0000.."); Wx(FlashSize()-1, 4); Ws(" ("); Wd(FlashSize(),1); Wsl(" bytes).");
Ws("EEPROM: 000.."); Wx(EepromSize()-1, 3); Ws(" ("); Wd(EepromSize(),1); Wsl(" bytes).");
// Dump uninterpreted fuse and lock bit values
ReadConfigBits(0, &cb); Ws("Fuses: low "); Wx(cb,2); Wflush();
ReadConfigBits(3, &hfuse); Ws(", high "); Wx(hfuse,2); Wflush();
ReadConfigBits(2, &efuse); Ws(", extended "); Wx(efuse,2); Wflush();
ReadConfigBits(1, &cb); Ws(", lock bits "); Wx(cb,2); Wsl(".");
// Interpret boot sector information
if (BootFlags()) {
int bootsize = 0;
int bootvec = 0;
switch (BootFlags()) {
case 1: bootsize = 128 << (3 - ((efuse/2) & 3)); bootvec = efuse & 1; break; // atmega88 & 168
case 2: bootsize = 256 << (3 - ((hfuse/2) & 3)); bootvec = hfuse & 1; break; // atmega328
default: Fail("Invalid BootFlags global data setting.");
}
bootsize *= 2; // Bootsize is in words but we report in bytes.
Ws(" Boot space: ");
Wx(FlashSize()-bootsize, 4); Ws(".."); Wx(FlashSize()-1, 4);
Ws(" ("); Wd(bootsize,1); Wsl(" bytes).");
Ws(" Vectors: ");
if (bootvec) Wsl("0."); else {Wx(FlashSize()-bootsize, 4); Wsl(".");}
}
}
void handle_client(int connfd)
{
char cmd[BUFSZ];
ssize_t r;
while (1) {
r = read_command(connfd, cmd, sizeof(cmd));
if (r < 1) {
Ws("Error reading command. Error code: "); Wd(r,1); Fail("!");
}
Ws("Got: "); Ws(cmd); Wl();
if (cmd[0] == 'k') break; // gdb quitting
handle_command(connfd, cmd);
}
Close((FileHandle)connfd);
return;
}
void DigisparkBreakAndSync(struct UPort *up) {
// Ws(" -- DigisparkBreakAndSync entry. "); WriteUPort(up);
for (int tries=0; tries<25; tries++) {
// Tell digispark to send a break and capture any returned pulse timings
int status = usb_control_msg(up->handle, OUT_TO_LW, 60, 33, 0, 0, 0, USB_TIMEOUT);
if (status < 0) {
if (status == -1) {
Wsl("Access denied sending USB message to Digispark. Run dwdebug as root, or add a udev rule such as");
Wsl("file /etc/udev/rules.d/60-usbtiny.rules containing:");
Wsl("SUBSYSTEM==\"usb\", ATTR{idVendor}==\"1781\", ATTR{idProduct}==\"0c9f\", GROUP=\"plugdev\", MODE=\"0666\"");
Wsl("And make sure that you are a member of the plugdev group.");
} else {
Ws("Digispark did not accept 'break and capture' command. Error code "); Wd(status,1); Wsl(".");
}
PortFail(up, "");
} else {
delay(120); // Wait while digispark sends break and reads back pulse timings
if (SetDwireBaud(up)) {return;}
}
Wc('.'); Wflush();
}
Wl(); PortFail(up, "Digispark/LittleWire/UsbTiny could not capture pulse timings after 25 break attempts.");
}
OutputInfo RBM::initialize(std::vector<double*>& parameterPointers,
std::vector<double*>& parameterDerivativePointers)
{
if(backprop)
{
for(int j = 0; j < H; j++)
{
for(int i = 0; i < D; i++)
{
parameterPointers.push_back(&W(j, i));
parameterDerivativePointers.push_back(&Wd(j, i));
}
}
for(int j = 0; j < H; j++)
{
parameterPointers.push_back(&bh(j));
parameterDerivativePointers.push_back(&bhd(j));
}
}
OutputInfo info;
info.dimensions.push_back(H);
return info;
}
void disassembler::Wss(const x86_insn *insn) { Wd(insn); }
void Wreg(int n) {
Wc('r'); Wd(n, 1);
}
/*This determines what material should leave through each outlet,
and rate of change of the contents resulting from the flow, of material,
out of each outlet of the surge unit.
*/
void CentrifugeMB::EvalProducts(CNodeEvalIndex & NEI)
{
StkSpConduit Sd("Sd", chLINEID(), this);
StkSpConduit Wd("Wd", chLINEID(), this);
SpConduit Dummy1;
SpConduit Dummy2;
switch (SolveMethod())
{
case SM_Direct:
{
//RqdSolidsToFiltrate = Range(1.0e-8, RqdSolidsToFiltrate, 0.3);
RqdSolidsToFiltrate = Range(0.0, RqdSolidsToFiltrate, 1.0);
RqdCakeMoist = Range(0.0, RqdCakeMoist, 0.99);
WashEff = Range(0.01, WashEff, 1.0);
const int iCl = IOWithId_Self(ioidLiq);
const int iCs = IOWithId_Self(ioidSol);
SpConduit & Cl = *IOConduit(iCl);
SpConduit & Cs = *IOConduit(iCs);
const int iWw = IOWithId_Self(ioidwash);
const int iW2 = IOWithId_Self(ioidwsh2);
const int iFW = IOWithId_Self(ioidFW);
const int iSW = IOWithId_Self(ioidSW);
SpConduit & QWw = (iWw >= 1 ? *IOConduit(iWw) : Dummy1);
SpConduit & QW2 = (iW2 >= 1 ? *IOConduit(iW2) : Dummy2);
SigmaQInPMin(QFd, som_ALL, Id_2_Mask(ioidFd));
const bool HasFeed = (QFd.QMass(som_ALL)>UsableMass);
const double TtlWashSol = QWw.QMass(som_Sol) + QW2.QMass(som_Sol);
//double CFeed = QFd.SpecieConc(QFd.Temp(), iScanEffSpecie, som_Liq);
double CFeed = QFd.SpecieConc(QFd.Temp(), iWashEffSpecie, som_Liq);
double CWash1 = QWw.SpecieConc(QWw.Temp(), iWashEffSpecie, som_Liq);
double CWash2 = QW2.SpecieConc(QW2.Temp(), iWashEffSpecie, som_Liq);
bool FeedLiqLow = false;
double CCake1 = 0.0;
if (fOn/* && HasFeed*/)
{
m_RB.EvalProducts(QFd);
m_EHX.EvalProducts(QFd);
const double MSol = QFd.QMass(som_Sol);
const double MLiq = QFd.QMass(som_Liq);
const double wash1 = QWw.QMass(som_Liq);
const double wash2 = QW2.QMass(som_Liq);
const double TtlLiqIn = MLiq + wash1 + wash2;
//will not achieve requirements if there are any solids in wash!!!
const double MSol2Filt = MSol * RqdSolidsToFiltrate;
const double RSol = MSol - MSol2Filt;
double MLiq2Cake = (RqdCakeMoist * RSol)/(1 - RqdCakeMoist);
FeedLiqLow = (TtlLiqIn<MLiq2Cake);
SetCI(1, /*fOn && */HasFeed && FeedLiqLow);
if (FeedLiqLow)
{//force all liquids to cake
MLiq2Cake = TtlLiqIn;
}
const double TotLiq = max(TtlLiqIn - MLiq2Cake, 1e-6);
const double Sol2Filt = GEZ(MLiq - MLiq2Cake) * MSol2Filt/TotLiq;
Cs.QSetM(QFd, som_Sol, RSol, IOP_Self(iCs));
Cs.QAddM(QWw, som_Sol, QWw.QMass(som_Sol));
Cs.QAddM(QW2, som_Sol, QW2.QMass(som_Sol));
Cl.QSetM(QFd, som_Liq, GEZ(MLiq - MLiq2Cake), IOP_Self(iCl));
Cl.QAddM(QFd, som_Sol, Sol2Filt);
//=======================================================
if ((MSol > 1e-6) && (MLiq > 1e-6))
{
double Cf, Cw1, Cw2, e1, e2, w1, w2;
w1 = min(MLiq2Cake*WashEff, wash1);
w2 = min(MLiq2Cake*WashEff, wash2);
e1 = w1/max(MLiq2Cake,1e-6);
e2 = w2/max(MLiq2Cake,1e-6);
// The liquid composition of the solids.
Cf = MLiq2Cake * (1.0 - e1 - e2 + e1*e2);
Cw1 = w1 - w2*e1;
Cw2 = w2;
Cs.QAddM(QFd, som_Liq, Cf);
Cs.QAddM(QWw, som_Liq, Cw1);
CCake1 = Cs.SpecieConc(Cs.Temp(), iWashEffSpecie, som_Liq);
Cs.QAddM(QW2, som_Liq, Cw2);
// The filtrate and washing streams
double Ff, Fw1, Fw2;
Ff = MLiq2Cake - Cf;
Fw1 = wash1 - Cw1;
Fw2 = wash2 - Cw2;
if (iFW >= 1)
{
rSpConduit FW =*IOConduit(iFW);
if (iSW >= 1)
{//more than one washing output stream connected
double Sw1, Sw2;
rSpConduit SW =*IOConduit(iSW);
Sw1 = wash1 * MSol2Filt/TotLiq;
Sw2 = wash2 * MSol2Filt/TotLiq;
FW.QSetM(QFd, som_Liq, w1, IOP_Self(iFW));
FW.QAddM(QWw, som_Liq, wash1 - w1);
FW.QAddM(QFd, som_Sol, Sw1);
SW.QSetM(QFd, som_Liq, w2 - w1*e2, IOP_Self(iSW));
SW.QAddM(QWw, som_Liq, w1*e2);
SW.QAddM(QW2, som_Liq, Fw2);
SW.QAddM(QFd, som_Sol, Sw2);
}
else // everything goes to the first washings stream
{
FW.QSetM(QFd, som_Liq, Ff, IOP_Self(iFW));
FW.QAddM(QWw, som_Liq, Fw1);
FW.QAddM(QW2, som_Liq, Fw2);
FW.QAddM(QFd, som_Sol, MSol2Filt - Sol2Filt);
}
}
else // everything goes to the filtrate
{
Cl.QAddM(QFd, som_Liq, Ff);
Cl.QAddM(QWw, som_Liq, Fw1);
Cl.QAddM(QW2, som_Liq, Fw2);
Cl.QAddM(QFd, som_Sol, MSol2Filt - Sol2Filt);
}
}
else
{
Cs.QAddM(QFd, som_Sol, MSol2Filt);
Cl.QAddM(QFd, som_Liq, MLiq2Cake);
Cl.QAddM(QWw, som_Liq, wash1);
Cl.QAddM(QW2, som_Liq, wash2);
}
}
else
{//off
SigmaQInPMin(Cs, som_ALL, Id_2_Mask(ioidFd));
SigmaQInPMin(Cl, som_ALL, Id_2_Mask(ioidwash)|Id_2_Mask(ioidwsh2));
if (iFW >= 1)
{
rSpConduit FW =*IOConduit(iFW);
FW.QZero();
}
if (iSW >= 1)
{
rSpConduit SW =*IOConduit(iSW);
SW.QZero();
}
}
ActCakeLiq = Cs.MassFrac(som_Liq);
ActCakeSolids = Cs.MassFrac(som_Sol);
ActFiltSolids = Cl.MassFrac(som_Sol);
ActFiltSolConc = Cl.PhaseConc(C_2_K(25.0), som_Sol, som_ALL);
ActFiltSolConcT = Cl.PhaseConc(Cl.Temp(), som_Sol, som_ALL);
//double Cuf = Cs.SpecieConc(Cs.Temp(), iScanEffSpecie, som_Liq);
//double Cof = Cl.SpecieConc(Cl.Temp(), iScanEffSpecie, som_Liq);
//ActScandrettEff = (CFeed - Cuf)/NZ(CFeed - Cof);
const double CCake2 = Cs.SpecieConc(Cs.Temp(), iWashEffSpecie, som_Liq);
dSpWashEff = (CFeed - CCake1)/NZ(CFeed - CWash1);
dSpWashEff2 = (CFeed - CCake2)/NZ(CFeed - CWash2);
const double CMErr = fabs(ActCakeLiq - RqdCakeMoist); //CakeMoisture error
const bool SolInWashErr = (CMErr > 1.0e-5 && TtlWashSol>1.0e-6);
SetCI(2, fOn && /*bTrackStatus && */HasFeed && !FeedLiqLow && !SolInWashErr && CMErr > 1.0e-5);
SetCI(3, fOn && /*bTrackStatus && */HasFeed && SolInWashErr);
break;
}
case SM_Inline:
case SM_Buffered:
{
Contents.ZeroDeriv();
//double CFeed = Sd().SpecieConc(Sd().Temp(), iScanEffSpecie, som_Liq);
//double CFeed = Sd().SpecieConc(Sd().Temp(), iWashEffSpecie, som_Liq);
//double CWash1 = QWw.SpecieConc(QWw.Temp(), iWashEffSpecie, som_Liq);
//double CWash2 = QW2.SpecieConc(QW2.Temp(), iWashEffSpecie, som_Liq);
m_RB.EvalProducts(Sd());
m_EHX.EvalProducts(Sd());
double SolMass = Contents.Mass(som_Sol);
double LiqMass = Contents.Mass(som_Liq);
//RqdSolidsToFiltrate = Range(1.0e-8, RqdSolidsToFiltrate, 0.3);
RqdSolidsToFiltrate = Range(0.0, RqdSolidsToFiltrate, 1.0);
RqdCakeMoist = Range(0.0, RqdCakeMoist, 1.0);
double CakeSol, LiqSol;
LiqSol = RqdSolidsToFiltrate / GTZ(1.0 - RqdSolidsToFiltrate);
CakeSol = (1.0 - RqdCakeMoist) / GTZ(RqdCakeMoist);
if (SolMass>1.0e-12 && LiqMass>1.0e-12)
{
SetProdMakeup(PMU_IOId | PMU_SLRatio, ioidLiq, Contents, LiqSol);
SetProdMakeup(PMU_IOId | PMU_SLRatio, ioidSol, Contents, CakeSol);
}
else
{
// Just Operate as a Tank
}
SigmaQInPMin(m_QFeed, som_ALL, First64IOIds);
EvalProducts_SurgeLevel(SolveInlineMethod(), false, ContentHgtOrd/*, &m_QFeed*/);
if (NoProcessJoins()>=1)
Xfer_EvalProducts(0, Joins[0].Pressure(), NULL, &m_QProdSrg, NULL, NULL, NULL);
ActCakeLiq = IOConduit(IOWithId_Self(ioidSol))->MassFrac(som_Liq);
ActCakeSolids = IOConduit(IOWithId_Self(ioidSol))->MassFrac(som_Sol);
ActFiltSolids = IOConduit(IOWithId_Self(ioidSol))->MassFrac(som_Sol);
ActFiltSolConc = IOConduit(IOWithId_Self(ioidSol))->PhaseConc(C_2_K(25.0), som_Sol, som_ALL);
ActFiltSolConcT = IOConduit(IOWithId_Self(ioidSol))->PhaseConc(IOConduit(IOWithId_Self(ioidSol))->Temp(), som_Sol, som_ALL);
//double Cuf = IOConduit(IOWithId_Self(ioidSol))->SpecieConc(IOConduit(IOWithId_Self(ioidSol))->Temp(), iScanEffSpecie, som_Liq);
//double Cof = IOConduit(IOWithId_Self(ioidSol))->SpecieConc(IOConduit(IOWithId_Self(ioidSol))->Temp(), iScanEffSpecie, som_Liq);
//ActScandrettEff = (CFeed - Cuf)/NZ(CFeed - Cof);
break;
}
}
}
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
RateThread(_period), commData(_commData), period(_period)
{
iCubHeadCenter eyeC(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// remove constraints on the links
// we use the chains for logging purpose
eyeL->setAllConstraints(false);
eyeR->setAllConstraints(false);
// release links
eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
}
// get the absolute reference frame of the head
Vector q(eyeC.getDOF(),0.0);
eyeCAbsFrame=eyeC.getH(q);
// ... and its inverse
invEyeCAbsFrame=SE3inv(eyeCAbsFrame);
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
bool ret;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
{
delete PrjL;
PrjL=Prj;
}
}
if (ret)
{
cxl=(*PrjL)(0,2);
cyl=(*PrjL)(1,2);
invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
}
else
PrjL=invPrjL=NULL;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
{
delete PrjR;
PrjR=Prj;
}
}
if (ret)
{
cxr=(*PrjR)(0,2);
cyr=(*PrjR)(1,2);
invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
}
else
PrjR=invPrjR=NULL;
Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
Vector N(1,10.0), Tt(1,1.0);
Matrix satLim(1,2);
satLim(0,0)=0.05;
satLim(0,1)=10.0;
pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
Vector z0(1,0.5);
pid->reset(z0);
dominantEye="left";
port_xd=NULL;
}
