// New style ctor
Fl_Roller::Fl_Roller(const char* l,int layout_size,Fl_Align layout_al,int label_w)
: Fl_Valuator(l,layout_size,layout_al,label_w)
{
step(.001);
}
void MyRobot::run()
{
// double compass_angle, compass_angle2;
double dist[NUM_DISTANCE_SENSOR];
for (int i=0; i<NUM_DISTANCE_SENSOR; i++) {
dist[i]=0.0;
}
//To go forward when robot starts
int flag=0;
while (step(_time_step) != -1) {
// Read the sensors
// const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
// compass_angle = convert_bearing_to_degrees(compass_val);
// Read the distances
for (int n=0; n<NUM_DISTANCE_SENSOR; n++) {
dist[n]=1000-(_distance_sensor[n]->getValue());
}
// Control logic of the robot
if (_mode == FORWARD) {
// Move forward
// When sufficiently close to a wall in front of robot,
// switch mode to wall following
if ((dist[1] < DISTANCE_LIMIT-150) || (dist[14] < DISTANCE_LIMIT-150)) {
_mode = WALL_FOLLOWER;
flag=1;
cout << "Mode " << WALL_FOLLOWER << ": Wall following mode activated" << endl;
}
else if (((dist[12] > DISTANCE_LIMIT-50) || (dist[11] > DISTANCE_LIMIT-50)) && (flag==1)){
_mode = TURN_RIGHT;
cout << "Turning right." << endl;
}
}
else {
// Wall following
if ((dist[1] < DISTANCE_LIMIT-150) || (dist[14] < DISTANCE_LIMIT-150)) {
_mode = WALL_FOLLOWER;
cout << "Backing up and turning left." << endl;
}
else {
//If the robots is turnning right
if (_mode == TURN_RIGHT) {
if ((dist[12] < DISTANCE_LIMIT-100) || (dist[11] < DISTANCE_LIMIT-100)){
_mode = TURN_LEFT;
cout << "Turning left." << endl;
}
else {
if ((dist[12] > DISTANCE_LIMIT-50) || (dist[11] > DISTANCE_LIMIT-50)){
_mode = TURN_RIGHT;
cout << "Turning right." << endl;
}
else {
_mode = FORWARD;
cout << "Moving forward." << endl;
}
}
}
//If the robot isn't turnnig right
else {
if ((dist[12] < DISTANCE_LIMIT-100) && (dist[11] < DISTANCE_LIMIT-100)){
_mode = TURN_LEFT;
cout << "Turning left." << endl;
}
else {
if ((dist[12] > DISTANCE_LIMIT-50) || (dist[11] > DISTANCE_LIMIT-50)){
_mode = TURN_RIGHT;
cout << "Turning right." << endl;
}
else {
_mode = FORWARD;
cout << "Moving forward." << endl;
}
}
}
}
}
// Send actuators commands according to the mode
switch (_mode){
case STOP:
_left_speed = 0;
_right_speed = 0;
break;
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case TURN_LEFT:
_left_speed = MAX_SPEED / 1.2;
_right_speed = MAX_SPEED;
break;
case TURN_RIGHT:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED / 1.2;
break;
case WALL_FOLLOWER:
_left_speed = -MAX_SPEED / 3.0;
_right_speed = -MAX_SPEED / 20.0;
break;
default:
break;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
/**
* @brief Tries to find a path from the given actor(-position) to a given target position
*
* Unlike Grid_CalcPathing, this function does not neccessarily calculate the TU values for
* all positions reachable from 'from'. Instead it tries to find the shortest/fastest path to
* the target position. There is no limit to maxTUs.
*
* @param[in] routing Reference to client or server side routing table (clMap, svMap)
* @param[in] actorSize The size of thing to calc the move for (e.g. size=2 means 2x2).
* The plan is to have the 'origin' in 2x2 units in the bottom-left (towards the lower coordinates) corner of the 2x2 square.
* @param[in,out] path Pointer to client or server side pathing table (clMap, svMap)
* @param[in] from The position to start the calculation from.
* @param[in] targetPos The position where we want to end up.
* @param[in] maxTUs The maximum TUs away from 'from' to calculate move-information for
* @param[in] crouchingState Whether the actor is currently crouching, 1 is yes, 0 is no.
* @param[in] fb_list Forbidden list (entities are standing at those points)
* @param[in] fb_length Length of forbidden list
* @sa G_MoveCalc
* @sa CL_ConditionalMoveCalc
*/
bool Grid_FindPath (const Routing &routing, const actorSizeEnum_t actorSize, pathing_t *path, const pos3_t from, const pos3_t targetPos, byte crouchingState, int maxTUs, byte ** fb_list, int fb_length)
{
bool found = false;
int count;
priorityQueue_t pqueue;
pos4_t epos; /**< Extended position; includes crouching state */
pos3_t pos;
/* this is the position of the current actor- so the actor can stand in the cell it is in when pathfinding */
pos3_t excludeFromForbiddenList;
/* Confirm bounds */
assert((from[2]) < PATHFINDING_HEIGHT);
assert(crouchingState == 0 || crouchingState == 1); /* s.a. ACTOR_MAX_STATES */
/* reset move data */
OBJSET(path->area, ROUTING_NOT_REACHABLE);
OBJSET(path->areaFrom, ROUTING_NOT_REACHABLE);
path->fblist = fb_list;
path->fblength = fb_length;
/* Prepare exclusion of starting-location (i.e. this should be ent-pos or le-pos) in Grid_CheckForbidden */
VectorCopy(from, excludeFromForbiddenList);
/* set starting position to 0 TUs.*/
RT_AREA_POS(path, from, crouchingState) = 0;
PQueueInitialise(&pqueue, 1024);
Vector4Set(epos, from[0], from[1], from[2], crouchingState);
PQueuePush(&pqueue, epos, 0);
count = 0;
while (!PQueueIsEmpty(&pqueue)) {
int dir;
PQueuePop(&pqueue, epos);
VectorCopy(epos, pos);
count++;
/* if reaching that square already took too many TUs,
* don't bother to reach new squares *from* there. */
const byte usedTUs = RT_AREA_POS(path, pos, crouchingState);
if (usedTUs >= maxTUs)
continue;
for (dir = 0; dir < PATHFINDING_DIRECTIONS; dir++) {
Step step(routing, pos, actorSize, crouchingState, dir);
/* Directions 12, 14, and 15 are currently undefined. */
if (dir == 12 || dir == 14 || dir == 15)
continue;
/* If this is a crouching or crouching move, forget it. */
if (dir == DIRECTION_STAND_UP || dir == DIRECTION_CROUCH)
continue;
if (!step.init())
continue; /* either dir is irrelevant or something worse happened */
if (step.isPossible(path)) {
/* Is this a better move into this cell? */
RT_AREA_TEST_POS(path, step.toPos, step.crouchingState);
if (RT_AREA_POS(path, step.toPos, step.crouchingState) <= step.TUsAfter) {
continue; /* This move is not optimum. */
}
/* Test for forbidden (by other entities) areas. */
/* Do NOT check the forbiddenList. We might find a multi-turn path. */
// if (Grid_CheckForbidden(excludeFromForbiddenList, step.actorSize, path, step.toPos[0], step.toPos[1], step.toPos[2])) {
// continue; /* That spot is occupied. */
// }
/* Store move in pathing table. */
Grid_SetMoveData(path, step.toPos, step.crouchingState, step.TUsAfter, step.dir, step.fromPos[2]);
pos4_t dummy;
int dist = step.TUsAfter + (int) (2 * VectorDist(step.toPos, targetPos));
Vector4Set(dummy, step.toPos[0], step.toPos[1], step.toPos[2], step.crouchingState);
PQueuePush(&pqueue, dummy, dist);
if (VectorEqual(step.toPos, targetPos)) {
found = true;
break;
}
}
}
if (found)
break;
}
/* Com_Printf("Loop: %i", count); */
PQueueFree(&pqueue);
return found;
}
void ncr5390_device::step(bool timeout)
{
UINT32 ctrl = scsi_bus->ctrl_r();
UINT32 data = scsi_bus->data_r();
UINT8 c = command[0] & 0x7f;
if(0)
logerror("%s: state=%d.%d %s\n",
tag(), state & STATE_MASK, (state & SUB_MASK) >> SUB_SHIFT,
timeout ? "timeout" : "change");
if(mode == MODE_I && !(ctrl & S_BSY)) {
state = IDLE;
istatus |= I_DISCONNECT;
reset_disconnect();
check_irq();
}
switch(state & SUB_MASK ? state & SUB_MASK : state & STATE_MASK) {
case IDLE:
break;
case ARB_COMPLETE << SUB_SHIFT: {
if(!timeout)
break;
int win;
for(win=7; win>=0 && !(data & (1<<win)); win--);
if(win != scsi_id) {
scsi_bus->data_w(scsi_refid, 0);
scsi_bus->ctrl_w(scsi_refid, 0, S_ALL);
fatalerror("need to wait for bus free\n");
break;
}
state = (state & STATE_MASK) | (ARB_ASSERT_SEL << SUB_SHIFT);
scsi_bus->ctrl_w(scsi_refid, S_SEL, S_SEL);
delay(6);
break;
}
case ARB_ASSERT_SEL << SUB_SHIFT:
if(!timeout)
break;
scsi_bus->data_w(scsi_refid, (1<<scsi_id) | (1<<bus_id));
state = (state & STATE_MASK) | (ARB_SET_DEST << SUB_SHIFT);
delay_cycles(4);
break;
case ARB_SET_DEST << SUB_SHIFT:
if(!timeout)
break;
state = (state & STATE_MASK) | (ARB_RELEASE_BUSY << SUB_SHIFT);
scsi_bus->ctrl_w(scsi_refid, c == CD_SELECT_ATN || c == CD_SELECT_ATN_STOP ? S_ATN : 0, S_ATN|S_BSY);
delay(2);
break;
case ARB_RELEASE_BUSY << SUB_SHIFT:
if(!timeout)
break;
if(ctrl & S_BSY) {
state = (state & STATE_MASK) | (ARB_DESKEW_WAIT << SUB_SHIFT);
if(c == CD_RESELECT)
scsi_bus->ctrl_w(scsi_refid, S_BSY, S_BSY);
delay_cycles(2);
} else {
state = (state & STATE_MASK) | (ARB_TIMEOUT_BUSY << SUB_SHIFT);
#ifdef DELAY_HACK
delay(1);
#else
delay(8192*select_timeout);
#endif
}
break;
case ARB_DESKEW_WAIT << SUB_SHIFT:
if(!timeout)
break;
scsi_bus->data_w(scsi_refid, 0);
scsi_bus->ctrl_w(scsi_refid, 0, S_SEL);
if(c == CD_RESELECT) {
logerror("%s: mode switch to Target\n", tag());
mode = MODE_T;
} else {
logerror("%s: mode switch to Initiator\n", tag());
mode = MODE_I;
}
state &= STATE_MASK;
step(true);
break;
case ARB_TIMEOUT_BUSY << SUB_SHIFT:
if(timeout) {
scsi_bus->data_w(scsi_refid, 0);
logerror("%s: select timeout\n", tag());
state = (state & STATE_MASK) | (ARB_TIMEOUT_ABORT << SUB_SHIFT);
delay(1000);
} else if(ctrl & S_BSY) {
state = (state & STATE_MASK) | (ARB_DESKEW_WAIT << SUB_SHIFT);
if(c == CD_RESELECT)
scsi_bus->ctrl_w(scsi_refid, S_BSY, S_BSY);
delay_cycles(2);
}
break;
case ARB_TIMEOUT_ABORT << SUB_SHIFT:
if(!timeout)
break;
if(ctrl & S_BSY) {
state = (state & STATE_MASK) | (ARB_DESKEW_WAIT << SUB_SHIFT);
if(c == CD_RESELECT)
scsi_bus->ctrl_w(scsi_refid, S_BSY, S_BSY);
delay_cycles(2);
} else {
scsi_bus->ctrl_w(scsi_refid, 0, S_ALL);
state = IDLE;
istatus |= I_DISCONNECT;
reset_disconnect();
check_irq();
}
break;
case SEND_WAIT_SETTLE << SUB_SHIFT:
if(!timeout)
break;
state = (state & STATE_MASK) | (SEND_WAIT_REQ_0 << SUB_SHIFT);
step(false);
break;
case SEND_WAIT_REQ_0 << SUB_SHIFT:
if(ctrl & S_REQ)
break;
state = state & STATE_MASK;
scsi_bus->data_w(scsi_refid, 0);
scsi_bus->ctrl_w(scsi_refid, 0, S_ACK);
step(false);
break;
case RECV_WAIT_REQ_1 << SUB_SHIFT:
if(!(ctrl & S_REQ))
break;
state = (state & STATE_MASK) | (RECV_WAIT_SETTLE << SUB_SHIFT);
delay_cycles(sync_period);
break;
case RECV_WAIT_SETTLE << SUB_SHIFT:
if(!timeout)
break;
if((state & STATE_MASK) != INIT_XFR_RECV_PAD)
fifo_push(scsi_bus->data_r());
scsi_bus->ctrl_w(scsi_refid, S_ACK, S_ACK);
state = (state & STATE_MASK) | (RECV_WAIT_REQ_0 << SUB_SHIFT);
step(false);
break;
case RECV_WAIT_REQ_0 << SUB_SHIFT:
if(ctrl & S_REQ)
break;
state = state & STATE_MASK;
step(false);
break;
case DISC_SEL_ARBITRATION:
if(c == CD_SELECT) {
state = DISC_SEL_WAIT_REQ;
command_length = derive_msg_size(fifo[0]);
} else
state = DISC_SEL_ATN_WAIT_REQ;
scsi_bus->ctrl_wait(scsi_refid, S_REQ, S_REQ);
if(ctrl & S_REQ)
step(false);
break;
case DISC_SEL_ATN_WAIT_REQ:
if(!(ctrl & S_REQ))
break;
if((ctrl & S_PHASE_MASK) != S_PHASE_MSG_OUT) {
function_complete();
break;
}
if(c == CD_SELECT_ATN)
scsi_bus->ctrl_w(scsi_refid, 0, S_ATN);
state = DISC_SEL_ATN_SEND_BYTE;
send_byte();
break;
case DISC_SEL_ATN_SEND_BYTE:
if(c == CD_SELECT_ATN_STOP) {
seq = 1;
function_complete();
} else {
command_length = derive_msg_size(fifo[0]);
state = DISC_SEL_WAIT_REQ;
}
break;
case DISC_SEL_WAIT_REQ:
if(!(ctrl & S_REQ))
break;
if((ctrl & S_PHASE_MASK) != S_PHASE_COMMAND) {
if(!command_length)
seq = 4;
scsi_bus->ctrl_wait(scsi_refid, 0, S_REQ);
function_bus_complete();
break;
}
if(seq < 3)
seq = 3;
state = DISC_SEL_SEND_BYTE;
send_byte();
break;
case DISC_SEL_SEND_BYTE:
if(command_length) {
command_length--;
if(!command_length)
seq = 4;
}
state = DISC_SEL_WAIT_REQ;
break;
case INIT_CPT_RECV_BYTE_ACK:
state = INIT_CPT_RECV_WAIT_REQ;
scsi_bus->ctrl_w(scsi_refid, 0, S_ACK);
break;
case INIT_CPT_RECV_WAIT_REQ:
if(!(ctrl & S_REQ))
break;
if((ctrl & S_PHASE_MASK) != S_PHASE_MSG_IN) {
command_pos = 0;
bus_complete();
} else {
state = INIT_CPT_RECV_BYTE_NACK;
recv_byte();
}
break;
case INIT_CPT_RECV_BYTE_NACK:
scsi_bus->ctrl_wait(scsi_refid, 0, S_REQ);
function_complete();
break;
case INIT_MSG_WAIT_REQ:
if((ctrl & (S_REQ|S_BSY)) == S_BSY)
break;
bus_complete();
break;
case INIT_XFR:
switch(xfr_phase) {
case S_PHASE_DATA_OUT:
dma_set(DMA_OUT);
if(tcount == 0 && fifo_pos == 1)
scsi_bus->ctrl_w(scsi_refid, 0, S_ATN);
state = INIT_XFR_SEND_BYTE;
send_byte();
break;
case S_PHASE_DATA_IN:
dma_set(DMA_IN);
state = tcount == fifo_pos+1 ?
INIT_XFR_RECV_BYTE_NACK : INIT_XFR_RECV_BYTE_ACK;
recv_byte();
break;
default:
logerror("%s: xfer on phase %d\n", tag(), scsi_bus->ctrl_r() & S_PHASE_MASK);
function_complete();
break;
}
break;
case INIT_XFR_WAIT_REQ:
if(!(ctrl & S_REQ))
break;
if((ctrl & S_PHASE_MASK) != xfr_phase) {
command_pos = 0;
bus_complete();
} else {
state = INIT_XFR;
step(false);
}
break;
case INIT_XFR_SEND_BYTE:
if(tcount == 0 && fifo_pos == 0)
bus_complete();
else
state = INIT_XFR_WAIT_REQ;
break;
case INIT_XFR_RECV_BYTE_ACK:
state = INIT_XFR_WAIT_REQ;
scsi_bus->ctrl_w(scsi_refid, 0, S_ACK);
break;
case INIT_XFR_RECV_BYTE_NACK:
function_complete();
break;
case INIT_XFR_SEND_PAD_WAIT_REQ:
if(!(ctrl & S_REQ))
break;
if((ctrl & S_PHASE_MASK) != xfr_phase) {
command_pos = 0;
bus_complete();
} else {
state = INIT_XFR_SEND_PAD;
send_byte();
}
break;
case INIT_XFR_SEND_PAD:
tcount--;
if(tcount) {
state = INIT_XFR_SEND_PAD_WAIT_REQ;
step(false);
} else
function_complete();
break;
case INIT_XFR_RECV_PAD_WAIT_REQ:
if(!(ctrl & S_REQ))
break;
if((ctrl & S_PHASE_MASK) != xfr_phase) {
command_pos = 0;
bus_complete();
} else {
state = INIT_XFR_RECV_PAD;
recv_byte();
}
break;
case INIT_XFR_RECV_PAD:
tcount--;
if(tcount) {
state = INIT_XFR_RECV_PAD_WAIT_REQ;
scsi_bus->ctrl_w(scsi_refid, 0, S_ACK);
step(false);
} else
function_complete();
break;
default:
logerror("%s: step() unexpected state %d.%d\n",
tag(),
state & STATE_MASK, (state & SUB_MASK) >> SUB_SHIFT);
exit(0);
}
}
nsresult txPatternParser::createStepPattern(txExprLexer& aLexer,
txIParseContext* aContext,
txPattern*& aPattern)
{
nsresult rv = NS_OK;
MBool isAttr = MB_FALSE;
Token* tok = aLexer.peek();
if (tok->mType == Token::AXIS_IDENTIFIER) {
if (TX_StringEqualsAtom(tok->Value(), nsGkAtoms::attribute)) {
isAttr = MB_TRUE;
}
else if (!TX_StringEqualsAtom(tok->Value(), nsGkAtoms::child)) {
// all done already for CHILD_AXIS, for all others
// XXX report unexpected axis error
return NS_ERROR_XPATH_PARSE_FAILURE;
}
aLexer.nextToken();
}
else if (tok->mType == Token::AT_SIGN) {
aLexer.nextToken();
isAttr = MB_TRUE;
}
tok = aLexer.nextToken();
txNodeTest* nodeTest;
if (tok->mType == Token::CNAME) {
// resolve QName
nsCOMPtr<nsIAtom> prefix, lName;
PRInt32 nspace;
rv = resolveQName(tok->Value(), getter_AddRefs(prefix), aContext,
getter_AddRefs(lName), nspace, PR_TRUE);
if (NS_FAILED(rv)) {
// XXX error report namespace resolve failed
return rv;
}
PRUint16 nodeType = isAttr ?
(PRUint16)txXPathNodeType::ATTRIBUTE_NODE :
(PRUint16)txXPathNodeType::ELEMENT_NODE;
nodeTest = new txNameTest(prefix, lName, nspace, nodeType);
if (!nodeTest) {
return NS_ERROR_OUT_OF_MEMORY;
}
}
else {
aLexer.pushBack();
rv = createNodeTypeTest(aLexer, &nodeTest);
NS_ENSURE_SUCCESS(rv, rv);
}
nsAutoPtr<txStepPattern> step(new txStepPattern(nodeTest, isAttr));
if (!step) {
delete nodeTest;
return NS_ERROR_OUT_OF_MEMORY;
}
rv = parsePredicates(step, aLexer, aContext);
NS_ENSURE_SUCCESS(rv, rv);
aPattern = step.forget();
return NS_OK;
}
void QStyleItem::initStyleOption()
{
QString type = elementType();
if (m_styleoption)
m_styleoption->state = 0;
switch (m_itemType) {
case Button: {
if (!m_styleoption)
m_styleoption = new QStyleOptionButton();
QStyleOptionButton *opt = qstyleoption_cast<QStyleOptionButton*>(m_styleoption);
opt->text = text();
opt->features = (activeControl() == "default") ?
QStyleOptionButton::DefaultButton :
QStyleOptionButton::None;
}
break;
case ItemRow: {
if (!m_styleoption)
m_styleoption = new QStyleOptionViewItemV4();
QStyleOptionViewItemV4 *opt = qstyleoption_cast<QStyleOptionViewItemV4*>(m_styleoption);
opt->features = 0;
if (activeControl() == "alternate")
opt->features |= QStyleOptionViewItemV2::Alternate;
}
break;
case Splitter: {
if (!m_styleoption) {
m_styleoption = new QStyleOption;
}
}
break;
case Item: {
if (!m_styleoption) {
m_styleoption = new QStyleOptionViewItemV4();
}
QStyleOptionViewItemV4 *opt = qstyleoption_cast<QStyleOptionViewItemV4*>(m_styleoption);
opt->features = QStyleOptionViewItemV4::HasDisplay;
opt->text = text();
opt->textElideMode = Qt::ElideRight;
QPalette pal = m_styleoption->palette;
pal.setBrush(QPalette::Base, Qt::NoBrush);
m_styleoption->palette = pal;
}
break;
case Header: {
if (!m_styleoption)
m_styleoption = new QStyleOptionHeader();
QStyleOptionHeader *opt = qstyleoption_cast<QStyleOptionHeader*>(m_styleoption);
opt->text = text();
opt->sortIndicator = activeControl() == "down" ?
QStyleOptionHeader::SortDown
: activeControl() == "up" ?
QStyleOptionHeader::SortUp : QStyleOptionHeader::None;
if (activeControl() == QLatin1String("beginning"))
opt->position = QStyleOptionHeader::Beginning;
else if (activeControl() == QLatin1String("end"))
opt->position = QStyleOptionHeader::End;
else if (activeControl() == QLatin1String("only"))
opt->position = QStyleOptionHeader::OnlyOneSection;
else
opt->position = QStyleOptionHeader::Middle;
}
break;
case ToolButton :{
if (!m_styleoption)
m_styleoption = new QStyleOptionToolButton();
QStyleOptionToolButton *opt =
qstyleoption_cast<QStyleOptionToolButton*>(m_styleoption);
opt->subControls = QStyle::SC_ToolButton;
opt->state |= QStyle::State_AutoRaise;
opt->activeSubControls = QStyle::SC_ToolButton;
}
break;
case ToolBar: {
if (!m_styleoption)
m_styleoption = new QStyleOptionToolBar();
}
break;
case Tab: {
if (!m_styleoption)
m_styleoption = new QStyleOptionTabV3();
QStyleOptionTabV3 *opt =
qstyleoption_cast<QStyleOptionTabV3*>(m_styleoption);
opt->text = text();
opt->shape = info() == "South" ? QTabBar::RoundedSouth : QTabBar::RoundedNorth;
if (activeControl() == QLatin1String("beginning"))
opt->position = QStyleOptionTabV3::Beginning;
else if (activeControl() == QLatin1String("end"))
opt->position = QStyleOptionTabV3::End;
else if (activeControl() == QLatin1String("only"))
opt->position = QStyleOptionTabV3::OnlyOneTab;
else
opt->position = QStyleOptionTabV3::Middle;
} break;
case Menu: {
if (!m_styleoption)
m_styleoption = new QStyleOptionMenuItem();
}
break;
case Frame: {
if (!m_styleoption)
m_styleoption = new QStyleOptionFrameV3();
QStyleOptionFrameV3 *opt = qstyleoption_cast<QStyleOptionFrameV3*>(m_styleoption);
opt->frameShape = QFrame::StyledPanel;
opt->lineWidth = 1;
opt->midLineWidth = 1;
}
break;
case TabFrame: {
if (!m_styleoption)
m_styleoption = new QStyleOptionTabWidgetFrameV2();
QStyleOptionTabWidgetFrameV2 *opt = qstyleoption_cast<QStyleOptionTabWidgetFrameV2*>(m_styleoption);
opt->shape = (info() == "South") ? QTabBar::RoundedSouth : QTabBar::RoundedNorth;
if (minimum())
opt->selectedTabRect = QRect(value(), 0, minimum(), height());
opt->tabBarSize = QSize(minimum() , height());
// oxygen style needs this hack
opt->leftCornerWidgetSize = QSize(value(), 0);
}
break;
case MenuItem:
case ComboBoxItem:
{
if (!m_styleoption)
m_styleoption = new QStyleOptionMenuItem();
QStyleOptionMenuItem *opt = qstyleoption_cast<QStyleOptionMenuItem*>(m_styleoption);
opt->checked = false;
opt->text = text();
opt->palette = widget()->palette();
}
break;
case CheckBox:
case RadioButton:
{
if (!m_styleoption)
m_styleoption = new QStyleOptionButton();
QStyleOptionButton *opt = qstyleoption_cast<QStyleOptionButton*>(m_styleoption);
if (!on())
opt->state |= QStyle::State_Off;
opt->text = text();
}
break;
case Edit: {
if (!m_styleoption)
m_styleoption = new QStyleOptionFrameV3();
QStyleOptionFrameV3 *opt = qstyleoption_cast<QStyleOptionFrameV3*>(m_styleoption);
opt->lineWidth = 1; // this must be non-zero
}
break;
case ComboBox :{
if (!m_styleoption)
m_styleoption = new QStyleOptionComboBox();
QStyleOptionComboBox *opt = qstyleoption_cast<QStyleOptionComboBox*>(m_styleoption);
opt->currentText = text();
}
break;
case SpinBox: {
if (!m_styleoption)
m_styleoption = new QStyleOptionSpinBox();
QStyleOptionSpinBox *opt = qstyleoption_cast<QStyleOptionSpinBox*>(m_styleoption);
opt->frame = true;
if (value() & 0x1)
opt->activeSubControls = QStyle::SC_SpinBoxUp;
else if (value() & (1<<1))
opt->activeSubControls = QStyle::SC_SpinBoxDown;
opt->subControls = QStyle::SC_All;
opt->stepEnabled = 0;
if (value() & (1<<2))
opt->stepEnabled |= QAbstractSpinBox::StepUpEnabled;
if (value() & (1<<3))
opt->stepEnabled |= QAbstractSpinBox::StepDownEnabled;
}
break;
case Slider:
case Dial:
{
if (!m_styleoption)
m_styleoption = new QStyleOptionSlider();
QStyleOptionSlider *opt = qstyleoption_cast<QStyleOptionSlider*>(m_styleoption);
opt->minimum = minimum();
opt->maximum = maximum();
// ### fixme - workaround for KDE inverted dial
opt->sliderPosition = value();
opt->singleStep = step();
if (opt->singleStep)
{
qreal numOfSteps = (opt->maximum - opt->minimum) / opt->singleStep;
// at least 5 pixels between tick marks
if (numOfSteps && (width() / numOfSteps < 5))
opt->tickInterval = qRound((5*numOfSteps / width()) + 0.5)*step();
else
opt->tickInterval = opt->singleStep;
}
else // default Qt-components implementation
opt->tickInterval = opt->maximum != opt->minimum ? 1200 / (opt->maximum - opt->minimum) : 0;
if (style() == QLatin1String("oxygen") && type == QLatin1String("dial"))
opt->sliderValue = maximum() - value();
else
opt->sliderValue = value();
opt->subControls = QStyle::SC_SliderGroove | QStyle::SC_SliderHandle;
opt->tickPosition = (activeControl() == "below") ?
QSlider::TicksBelow : (activeControl() == "above" ?
QSlider::TicksAbove:
QSlider::NoTicks);
if (opt->tickPosition != QSlider::NoTicks)
opt->subControls |= QStyle::SC_SliderTickmarks;
opt->activeSubControls = QStyle::SC_None;
}
break;
case ProgressBar: {
if (QProgressBar *bar= qobject_cast<QProgressBar*>(widget())){
bar->setMaximum(maximum());
bar->setMinimum(minimum());
if (maximum() != minimum())
bar->setValue(1);
}
if (!m_styleoption)
m_styleoption = new QStyleOptionProgressBarV2();
QStyleOptionProgressBarV2 *opt = qstyleoption_cast<QStyleOptionProgressBarV2*>(m_styleoption);
opt->orientation = horizontal() ? Qt::Horizontal : Qt::Vertical;
opt->minimum = minimum();
opt->maximum = maximum();
opt->progress = value();
}
break;
case GroupBox: {
if (!m_styleoption)
m_styleoption = new QStyleOptionGroupBox();
QStyleOptionGroupBox *opt = qstyleoption_cast<QStyleOptionGroupBox*>(m_styleoption);
opt->text = text();
opt->lineWidth = 1;
opt->subControls = QStyle::SC_GroupBoxLabel;
if (sunken()) // Qt draws an ugly line here so I ignore it
opt->subControls |= QStyle::SC_GroupBoxFrame;
else
opt->features |= QStyleOptionFrameV2::Flat;
if (activeControl() == "checkbox")
opt->subControls |= QStyle::SC_GroupBoxCheckBox;
if (QGroupBox *group= qobject_cast<QGroupBox*>(widget())) {
group->setTitle(text());
group->setCheckable(opt->subControls & QStyle::SC_GroupBoxCheckBox);
}
}
break;
case ScrollBar: {
if (!m_styleoption)
m_styleoption = new QStyleOptionSlider();
QStyleOptionSlider *opt = qstyleoption_cast<QStyleOptionSlider*>(m_styleoption);
opt->minimum = minimum();
opt->maximum = maximum();
opt->pageStep = horizontal() ? width() : height();
opt->orientation = horizontal() ? Qt::Horizontal : Qt::Vertical;
opt->sliderPosition = value();
opt->sliderValue = value();
opt->activeSubControls = (activeControl() == QLatin1String("up"))
? QStyle::SC_ScrollBarSubLine :
(activeControl() == QLatin1String("down")) ?
QStyle::SC_ScrollBarAddLine:
QStyle::SC_ScrollBarSlider;
opt->sliderValue = value();
opt->subControls = QStyle::SC_All;
QScrollBar *bar = qobject_cast<QScrollBar *>(widget());
bar->setMaximum(maximum());
bar->setMinimum(minimum());
bar->setValue(value());
}
break;
default:
break;
}
if (!m_styleoption)
m_styleoption = new QStyleOption();
m_styleoption->rect = QRect(m_paintMargins, m_paintMargins, width() - 2* m_paintMargins, height() - 2 * m_paintMargins);
if (isEnabled())
m_styleoption->state |= QStyle::State_Enabled;
if (m_active)
m_styleoption->state |= QStyle::State_Active;
if (m_sunken)
m_styleoption->state |= QStyle::State_Sunken;
if (m_raised)
m_styleoption->state |= QStyle::State_Raised;
if (m_selected)
m_styleoption->state |= QStyle::State_Selected;
if (m_focus)
m_styleoption->state |= QStyle::State_HasFocus;
if (m_on)
m_styleoption->state |= QStyle::State_On;
if (m_hover)
m_styleoption->state |= QStyle::State_MouseOver;
if (m_horizontal)
m_styleoption->state |= QStyle::State_Horizontal;
if (widget()) {
widget()->ensurePolished();
if (type == QLatin1String("tab") && style() != QLatin1String("mac")) {
// Some styles actually check the beginning and end position
// using widget geometry, so we have to trick it
widget()->setGeometry(0, 0, width(), height());
if (activeControl() != "beginning")
m_styleoption->rect.translate(1, 0); // Don't position at start of widget
if (activeControl() != "end")
widget()->resize(200, height());
}
#ifdef Q_OS_WIN
else widget()->resize(width(), height());
#endif
widget()->setEnabled(isEnabled());
m_styleoption->fontMetrics = widget()->fontMetrics();
if (!m_styleoption->palette.resolve())
m_styleoption->palette = widget()->palette();
if (m_hint.contains("mac.mini")) {
widget()->setAttribute(Qt::WA_MacMiniSize);
} else if (m_hint.contains("mac.small")) {
widget()->setAttribute(Qt::WA_MacSmallSize);
}
}
}
/*!
Set the maximum value of the range
\param value Maximum value
\sa setMinValue(), maxVal()
*/
void QwtCounter::setMaxValue( double value )
{
setRange( minValue(), value, step() );
}
bool SequentialSolver::LocalSubKKT::projected_primal_and_bilateral( AssembledSystem& res,
const AssembledSystem& sys,
real eps,
bool only_lower)
{
scoped::timer step("subsystem primal-bilateral");
projection_basis(P, sys.P, sys.isPIdentity);
if(sys.n)
{
unsigned nb_bilaterals = projection_bilateral( Q, Q_unil, sys );
if( !nb_bilaterals ) // no bilateral constraints
{
res.H = rmat();
return false;
}
else
{
filter_kkt(res.H, sys.H, P, Q, sys.J, sys.C, eps, sys.isPIdentity, nb_bilaterals == sys.n, only_lower);
res.dt = sys.dt;
res.m = res.H.rows();
res.n = Q_unil.cols();
res.P.resize( res.m, res.m );
res.P.setIdentity();
res.isPIdentity = true;
if( res.n ) // there are non-bilat constraints
{
// keep only unilateral constraints in C
res.C.resize( res.n, res.n );
res.C = Q_unil.transpose() * sys.C * Q_unil;
// compute J_unil and resize it
res.J.resize( res.n, res.m );
static rmat tmp; // try to improve matrix allocation
tmp = Q_unil.transpose() * sys.J * P;
for( rmat::Index i = 0; i < tmp.rows(); ++i)
{
res.J.startVec( i );
for(rmat::InnerIterator it(tmp, i); it; ++it) {
res.J.insertBack(i, it.col()) = it.value();
}
}
res.J.finalize();
}
else
{
res.J = rmat();
res.C = rmat();
}
return true;
}
}
else // no constraints
{
res.H = rmat();
return false;
}
}
void step(void)
{
step(next_time());
}
void RepoQuery::exec() {
step();
reset();
}
void BaseSequentialSolver::solve_impl(vec& res,
const system_type& sys,
const vec& rhs,
bool correct) const {
assert( response );
// reset bench if needed
if( this->bench ) {
bench->clear();
bench->restart();
}
// free velocity
vec free_res( sys.m );
sub.solve(*response, free_res, rhs.head( sys.m ));
// we're done
if( blocks.empty() )
{
res = free_res;
return;
}
// lagrange multipliers TODO reuse res.tail( sys.n ) ?
vec lambda = res.tail(sys.n);
// net constraint velocity correction
vec net = mapping_response * lambda;
// lambda change work vector
vec delta = vec::Zero(sys.n);
// lcp rhs
vec constant = rhs.tail(sys.n) - JP * free_res.head(sys.m);
// lcp error
vec error = vec::Zero(sys.n);
const real epsilon = relative.getValue() ?
constant.norm() * precision.getValue() : precision.getValue();
if( this->bench ) this->bench->lcp(sys, constant, *response, lambda);
// outer loop
unsigned k = 0, max = iterations.getValue();
// vec primal;
for(k = 0; k < max; ++k) {
real estimate2 = step( lambda, net, sys, constant, error, delta, correct );
if( this->bench ) this->bench->lcp(sys, constant, *response, lambda);
// stop if we only gain one significant digit after precision
if( std::sqrt(estimate2) / sys.n <= epsilon ) break;
}
res.head( sys.m ) = free_res + net;
res.tail( sys.n ) = lambda;
if( this->f_printLog.getValue() )
serr << "iterations: " << k << ", (abs) residual: " << (net - mapping_response * lambda).norm() << sendl;
}
// only called on PE 0
void HbmLB::reportLBQulity(double mload, double mCpuLoad, double totalload, int nmsgs, double bytes)
{
static int pecount=0;
CmiAssert(CkMyPe() == 0);
if (mload > maxLoad) maxLoad = mload;
if (mCpuLoad > maxCpuLoad) maxCpuLoad = mCpuLoad;
totalLoad += totalload;
maxCommCount += nmsgs;
maxCommBytes += bytes; // KB
pecount++;
if (pecount == CkNumPes()) {
CkPrintf("[%d] Load Summary: max (with comm): %f max (obj only): %f total: %f at step %d nonlocal: %d msgs, %.2fKB reported from %d PEs.\n", CkMyPe(), maxLoad, maxCpuLoad, totalLoad, step(), maxCommCount, maxCommBytes, pecount);
maxLoad = 0.0;
maxCpuLoad = 0.0;
totalLoad = 0.0;
maxCommCount = 0;
maxCommBytes = 0.0;
pecount = 0;
}
}
//*****************************************************************************
// Vp_Value_Input_Spin::handle( event) -- Event handler
int Vp_Value_Input_Spin::handle( int event)
{
double v;
int olddelta;
int mx = Fl::event_x();
int my = Fl::event_y();
int sxx = x(), syy = y(), sww = w(), shh = h();
sxx += sww - buttonssize(); sww = buttonssize();
if(!indrag && ( !sldrag || !((mx>=sxx && mx<=(sxx+sww)) &&
(my>=syy && my<=(syy+shh)))) ) {
indrag=0;
switch(event) {
case FL_PUSH:
case FL_DRAG:
sldrag=1;
break;
case FL_FOCUS:
input.take_focus();
break;
case FL_UNFOCUS:
redraw();
break;
case FL_KEYDOWN:
if (Fl::event_key(FL_Up)) {
v = increment(value(),1);
handle_drag(soft()?softclamp(v):clamp(v));
return 1;
}
if (Fl::event_key(FL_Down)) {
v = increment(value(),-1);
handle_drag(soft()?softclamp(v):clamp(v));
return 1;
}
break;
default:
sldrag=0;
}
input.type(step()>=1.0 ? FL_INT_INPUT : FL_FLOAT_INPUT);
return input.handle(event);
}
switch (event) {
case FL_PUSH:
// if (!step()) goto DEFAULT;
iy = my;
ix = mx;
drag = Fl::event_button();
handle_push();
indrag=1;
mouseobj=1;
Fl::add_timeout(.25, repeat_callback, this);
delta=0;
if(Fl::event_inside(sxx,syy,sww,shh/2)) {
deltadir=1;
} else if (Fl::event_inside(sxx,syy+shh/2,sww,shh/2)) {
deltadir=-1;
} else {
deltadir=0;
}
increment_cb();
redraw();
return 1;
case FL_DRAG:
if(mouseobj) {
mouseobj=0;
Fl::remove_timeout(repeat_callback, this);
}
// if (!step()) goto DEFAULT;
olddelta=delta;
delta = - (Fl::event_y()-iy);
if ((delta>1) || (delta<-1) ) { deltadir=((olddelta-delta)>0)?-1:(((olddelta-delta)<0)?1:0); }
else { deltadir=0; delta = olddelta;}
switch (drag) {
case 3: v = increment(value(), deltadir*100); break;
case 2: v = increment(value(), deltadir*10); break;
default:v = increment(value(), deltadir); break;
}
v = round(v);
handle_drag(soft()?softclamp(v):clamp(v));
indrag=1;
return 1;
case FL_RELEASE:
if(mouseobj) {
Fl::remove_timeout(repeat_callback, this);
}
// if (!step()) goto DEFAULT;
indrag=0;
delta=0;
deltadir=0;
mouseobj=0;
handle_release();
redraw();
return 1;
case FL_FOCUS:
indrag=0;
return input.take_focus();
default:
indrag=0;
input.type(step()>=1.0 ? FL_INT_INPUT : FL_FLOAT_INPUT);
return 1;
}
}
void Fl_Roller::draw()
{
if (damage()&(FL_DAMAGE_ALL|FL_DAMAGE_HIGHLIGHT)) draw_box();
int X=0; int Y=0; int W=w(); int H=h(); box()->inset(X,Y,W,H);
if (W<=0 || H<=0) return;
double s = step();
if (!s) s = (maximum()-minimum())/100;
int offset = int(value()/s);
const double ARC = 1.5; // 1/2 the number of radians visible
const double delta = .2; // radians per knurl
if (type()==HORIZONTAL)
{
// draw shaded ends of wheel:
int h1 = W/4+1; // distance from end that shading starts
fl_color(button_color()); fl_rectf(X+h1,Y,W-2*h1,H);
for (int i=0; h1; i++)
{
fl_color((Fl_Color)(FL_GRAY-i-1));
int h2 = FL_GRAY-i-1 > FL_DARK3 ? 2*h1/3+1 : 0;
fl_rectf(X+h2,Y,h1-h2,H);
fl_rectf(X+W-h1,Y,h1-h2,H);
h1 = h2;
}
if (active_r())
{
// draw ridges:
double junk;
for (double y = -ARC+modf(offset*sin(ARC)/(W/2)/delta,&junk)*delta;;
y += delta)
{
int y1 = int((sin(y)/sin(ARC)+1)*W/2);
if (y1 <= 0) continue; else if (y1 >= W-1) break;
fl_color(FL_DARK3); fl_line(X+y1,Y+1,X+y1,Y+H-1);
if (y < 0) y1--; else y1++;
fl_color(FL_LIGHT1);fl_line(X+y1,Y+1,X+y1,Y+H-1);
}
// draw edges:
h1 = W/8+1; // distance from end the color inverts
fl_color(FL_DARK2);
fl_line(X+h1,Y+H-1,X+W-h1,Y+H-1);
fl_color(FL_DARK3);
fl_line(X,Y+H,X,Y);
fl_line(X,Y,X+h1,Y);
fl_line(X+W-h1,Y,X+W,Y);
fl_color(FL_LIGHT2);
fl_line(X+h1,Y,X+W-h1,Y);
fl_line(X+W,Y,X+W,Y+H);
fl_line(X+W,Y+H,X+W-h1,Y+H);
fl_line(X+h1,Y+H,X,Y+H);
}
} // vertical one
else
{
offset = (1-offset);
// draw shaded ends of wheel:
int h1 = H/4+1; // distance from end that shading starts
fl_color(button_color()); fl_rectf(X,Y+h1,W,H-2*h1);
for (int i=0; h1; i++)
{
fl_color((Fl_Color)(FL_GRAY-i-1));
int h2 = FL_GRAY-i-1 > FL_DARK3 ? 2*h1/3+1 : 0;
fl_rectf(X,Y+h2,W,h1-h2);
fl_rectf(X,Y+H-h1,W,h1-h2);
h1 = h2;
}
if (active_r())
{
// draw ridges:
double junk;
for (double y = -ARC+modf(offset*sin(ARC)/(H/2)/delta,&junk)*delta;
; y += delta)
{
int y1 = int((sin(y)/sin(ARC)+1)*H/2);
if (y1 <= 0) continue; else if (y1 >= H-1) break;
fl_color(FL_DARK3); fl_line(X+1,Y+y1,X+W-1,Y+y1);
if (y < 0) y1--; else y1++;
fl_color(FL_LIGHT1);fl_line(X+1,Y+y1,X+W-1,Y+y1);
}
// draw edges:
h1 = H/8+1; // distance from end the color inverts
fl_color(FL_DARK2);
fl_line(X+W-1,Y+h1,X+W-1,Y+H-h1);
fl_color(FL_DARK3);
fl_line(X+W,Y,X,Y);
fl_line(X,Y,X,Y+h1);
fl_line(X,Y+H-h1,X,Y+H);
fl_color(FL_LIGHT2);
fl_line(X,Y+h1,X,Y+H-h1);
fl_line(X,Y+H,X+W,Y+H);
fl_line(X+W,Y+H,X+W,Y+H-h1);
fl_line(X+W,Y+h1,X+W,Y);
}
}
if (focused())
{
focus_box()->draw(0,0,w(),h(), FL_BLACK, FL_INVISIBLE);
}
}
/* CS_PLSI , based on Probabilistic Latent Semantic Indexing, as described in
Hoffman 1999, on a sparse data matrix X for K many latent variables.
W := vector of N many words {w_1, w_2, ..., w_N}
D := vector of M many documents {d_1, d_2, ..., d_M}
X := the (word, document) co-occurrence matrix:
{x_nm} forall (n, m), where x_nm is 1 or 0
Z := the 'true topic' matrix:
{z_nm} forall (n, m), where each z_nm has 1-of-K representation. So, I'm
denoting p(z_nmk = 1) as p(z_k | w_n, d_m)
note that p(Z) is a Kx1 vector [p(z_1), p(z_2), ... p(z_K)] where p(z_k) is a
scalar, while p(Z | W, D) is an NxM matrix where each element p(Z | W, D)_nm
is a Kx1 vector p(Z | w_n, d_m).
TODO: Fix this description of p(Z | W, D)
p(Z | W, D) is realized as a NxMxK matrix where p(Z | W, D)_nmk = p(z_k| w_n,
d_m), following the definition of p(z_k | w_n, d_m) given above.
Initial values for p(Z), p(W | Z), and p(D | Z) are optional. If omitted,
they are randomly initialized.
required args:
cs X -- NxM data matrix over N words {w_n} and M docs {d_m}, where
X_nm indicates the number of occurrences of word w_n in
document d_m.
int K -- # latent variable states to solve for
double *pZout -|
double *pWgZout -+-- Allocated space to put the ML results
double *pDgZout -|
optional args:
double b -- "control paramater" beta, see eq. (9)
double *pZ -- p(Z) -- Kx1 vec; pZ(k) = p(z_k)
double *pWgZ -- p(W | Z) -- NxK mat; pWgZ[n, k] = p(w_n | z_k)
double *pDgZ -- p(D | Z) -- KxM mat; pDgZ[k, m] = p(d_m | z_k)
returns:
double *logLike -- The log likelihood of the solution
*/
double cs_plsi(const cs *X, const CS_INT K, const double B,
const double *pZin, const double *pWgZin, const double *pDgZin,
double *pZout, double *pWgZout, double *pDgZout) {
/* It is worth noting that pWgZ should be transposed, and be passed in
as a kxn matrix. pDgZ should be normal, a kxm matrix. This is done for
cache efficiency. */
bool debug = 0;
CS_INT n = X->n; /* Number of columns in X */
CS_INT m = X->m; /* Number of rows in X */
CS_INT k = 0;
double ll, llast = 0;
if(debug) printf("About to malloc\n");
double *pZt = cs_malloc(K, sizeof(double));
if(debug) printf("Malloc'd pZt\n");
double *pWgZt = cs_malloc(m*K, sizeof(double));
if(debug) printf("Malloc'd pWgZt\n");
double *pDgZt = cs_malloc(n*K, sizeof(double));
if(debug) printf("Malloc'd pDgZt\n");
if(debug) printf("About to memcpy\n");
memcpy(pZt, pZin, K * sizeof(double));
if(debug) printf("Memcpy'd pZ\n");
memcpy(pWgZt, pWgZin, K * m * sizeof(double));
if(debug) printf("Memcpy'd pWgZ\n");
memcpy(pDgZt, pDgZin, K * n * sizeof(double));
if(debug) printf("Memcpy'd pDgZ\n");
ll = step(X, K, B, pZt, pWgZt, pDgZt, pZout, pWgZout, pDgZout);
CS_INT iter = 0;
if(debug) printf("Got ll back, testing for convergance\n");
while(!converged(llast, ll)) {
if(debug) printf("About to memcpy\n");
memcpy(pZt, pZout, K * sizeof(double));
if(debug) printf("Memcpy'd pZ\n");
memcpy(pWgZt, pWgZout, K * m * sizeof(double));
if(debug) printf("Memcpy'd pWgZ\n");
memcpy(pDgZt, pDgZout, K * n * sizeof(double));
if(debug) printf("Memcpy'd pDgZ\n");
for ( k = 0; k < K; k++) {
pZout[k] = 0.0;
}
for ( k = 0; k < K*m; k++) {
pWgZout[k] = 0.0;
}
for( k = 0; k < K*n; k++) {
pDgZout[k] = 0.0;
}
llast = ll;
ll = step(X, K, B, pZt, pWgZt, pDgZt, pZout, pWgZout, pDgZout);
iter++;
/*if(iter % 100 == 0) {*/
printf("iter = %d\n", iter);
/*}*/
}
cs_free(pZt);
cs_free(pWgZt);
cs_free(pDgZt);
printf("iter = %d\n", iter);
return ll;
}
void step(char *body){
char in[100];
int takeinput=1;
int lastcommand=0;
while(takeinput){
scanf("%s",in);
//printf("in %s\n",in);
//printf("body %s\n",body);
if(!strcmp(in,"step")||!strcmp(in,"s")){
//take next step in program
lastcommand=1;
char nextstep[strlen(body)];
int i;
for(i=0;body[i]!=','&&body[i]!=0;i++){
nextstep[i]=body[i];
}
nextstep[i]=0;
if(!strncmp(nextstep,"//struct",8)){
//printf("body %s %d\n",body+i,strncmp(body+i,"//ends",6));
nextstep[i]=body[i];
while(strncmp(body+i,"//ends",6)){
//printf("loop\n");
nextstep[i]=body[i];
i++;
}
nextstep[i]=0;
body+=6;
}
if(!strncmp(nextstep,"//while",7)){
char * condition = nextstep+7;
char num[strlen(nextstep)];
int k=0;
//printf("condition number %s\n",condition);
while(*condition!=' '&&*condition){
num[k]=*condition;
condition++;
k++;
}
num[k]=0;
condition++;
char cond[strlen(nextstep)];
k=0;
while(*condition!=','&&*condition){
cond[k]=*condition;
condition++;
k++;
}
cond[k]=0;
//printf("while condition %s\n",cond);
executeStatement* e = new executeStatement(cond);
void * ret=e->execute();
//printf("ret %d\n",*(int *)ret);
char loopbody[strlen(body)];
loopbody[0]=0;
//strcat(loopbody,nextstep);
//loopbody[strlen(loopbody)]=',';
//loopbody[strlen(loopbody)+1]=0;
k=i+1;
int wbool=1;
while(wbool){
for(i=0;body[k]!=','&&body[k]!=0;k++,i++){
nextstep[i]=body[k];
//printf("i = %d body=%c\n",i,body[k]);
}
nextstep[i]=body[k];
k++;
i++;
nextstep[i]=0;
//printf("nextstep %s num %s\n",nextstep,num);
if(!strncmp(nextstep,"//endw",6)){
//printf("got 1\n");
if(!strncmp(nextstep+6,num,strlen(num))){
wbool=0;
//printf("got 2\n");
continue;
}
}
strcat(loopbody,nextstep);
}
loopbody[strlen(loopbody)-1]=0;
//printf("loopbody %s\n",loopbody);
if(*(int *)ret!=0){
step(loopbody);
}
else{
//printf("nextstep1251521 %s\nbody %s\nloopbody %s\n",nextstep,body,loopbody);
//printf("nextstep %d body %d loopbody %d i %d\n",strlen(nextstep),strlen(body),strlen(loopbody),i);
body+=i+strlen(loopbody)+strlen(nextstep)+strlen(cond)+3;
if(*body==0)
takeinput=0;
//printf("body %s\n",body);
}
//printf("body123 %s\n",body);
}
else if(!strncmp(nextstep,"//if",4)){
char * condition = nextstep+4;
char num[strlen(nextstep)];
int k=0;
//printf("condition number %s\n",condition);
while(*condition!=' '&&*condition){
num[k]=*condition;
condition++;
k++;
}
num[k]=0;
condition++;
char cond[strlen(nextstep)];
k=0;
while(*condition!=','&&*condition){
cond[k]=*condition;
condition++;
k++;
}
cond[k]=0;
//printf("if condition %s\n",cond);
executeStatement* e = new executeStatement(cond);
void * ret=e->execute();
//printf("ret %d\n",*(int *)ret);
if(*(int *)ret!=0){
char ifbody[strlen(body)];
ifbody[0]=0;
k=i+1;
int wbool=1;
int haselse=0;
while(wbool){
for(i=0;body[k]!=','&&body[k]!=0;k++,i++){
nextstep[i]=body[k];
//printf("i = %d body=%c\n",i,body[k]);
}
nextstep[i]=body[k];
k++;
i++;
nextstep[i]=0;
//printf("nextstep %s num %s\n",nextstep,num);
if(!strncmp(nextstep,"//endf",6)||!strncmp(nextstep,"//else",6)){
//printf("got 1\n");
if(!strncmp(nextstep+6,num,strlen(num))){
if(!strncmp(nextstep,"//else",6)){
haselse=1;
}
wbool=0;
//printf("got 2\n");
continue;
}
}
strcat(ifbody,nextstep);
}
ifbody[strlen(ifbody)-1]=0;
//printf("ifbody %s\n",ifbody);
step(ifbody);
//printf("body prejump %s\n",body);
body+=strlen(ifbody)+strlen(cond)+7+strlen(num)*2+6;
if(*body==',')
body++;
//printf("body ifsasdf %s\n %s\n%s\n",body,ifbody,cond);
if(haselse){
ifbody[0]=0;
k=0;
wbool=1;
//printf("body elsesasdf %s\n",body);
while(wbool){
for(i=0;body[k]!=','&&body[k]!=0;k++,i++){
nextstep[i]=body[k];
//printf("i = %d body=%c\n",i,body[k]);
}
nextstep[i]=body[k];
k++;
i++;
nextstep[i]=0;
//printf("nextstep %s num %s\n",nextstep,num);
if(!strncmp(nextstep,"//ende",6)){
//printf("got 1\n");
if(!strncmp(nextstep+6,num,strlen(num))){
wbool=0;
//printf("got 2\n");
continue;
}
}
strcat(ifbody,nextstep);
}
ifbody[strlen(ifbody)-1]=0;
//printf("bodypree jump elso %s\n",body);
body+=strlen(ifbody)+7+strlen(num);
if(*body==',')
body++;
//printf("body else1345 %s\n",body);
}
//body+=strlen(nextstep);
if(*body==0)
takeinput=0;
}
else{
char ifbody[strlen(body)];
ifbody[0]=0;
k=i+1;
int wbool=1;
int haselse=0;
while(wbool){
for(i=0;body[k]!=','&&body[k]!=0;k++,i++){
nextstep[i]=body[k];
//printf("i = %d body=%c\n",i,body[k]);
}
nextstep[i]=body[k];
k++;
i++;
nextstep[i]=0;
//printf("nextstep %s num %s\n",nextstep,num);
if(!strncmp(nextstep,"//endf",6)||!strncmp(nextstep,"//else",6)){
//printf("got 1\n");
if(!strncmp(nextstep+6,num,strlen(num))){
if(!strncmp(nextstep,"//else",6)){
haselse=1;
}
wbool=0;
//printf("got 2\n");
continue;
}
}
strcat(ifbody,nextstep);
}
ifbody[strlen(ifbody)-1]=0;
//printf("abody %s\n",body);
body+=strlen(ifbody)+strlen(cond)+7+strlen(num)*2+6;
//printf("body ifsasdf2365 %c\n",*body);
if(*body==',')
body++;
//printf("body ifsasdf %s\n",body);
if(haselse){
ifbody[0]=0;
k=0;
wbool=1;
//printf("body elsesasdf %s\n",body);
while(wbool){
for(i=0;body[k]!=','&&body[k]!=0;k++,i++){
nextstep[i]=body[k];
//printf("i = %d body=%c\n",i,body[k]);
}
nextstep[i]=body[k];
k++;
i++;
nextstep[i]=0;
//printf("nextstep %s num %s\n",nextstep,num);
if(!strncmp(nextstep,"//ende",6)){
//printf("got 1\n");
if(!strncmp(nextstep+6,num,strlen(num))){
wbool=0;
//printf("got 2\n");
continue;
}
}
strcat(ifbody,nextstep);
}
ifbody[strlen(ifbody)-1]=0;
//printf("elsebody %s\n",ifbody);
step(ifbody);
//printf("bodypree jump elso %s\n",body);
body+=strlen(ifbody)+7+strlen(num);
if(*body==',')
body++;
//printf("body else1345 %s\n",body);
}
if(*body==0)
takeinput=0;
}
}
else{
//printf("adding body\n");
if(body[i]!=0)
body+=i+1;
else//were done
takeinput=0;
executeStatement* e = new executeStatement(nextstep);
e->execute();
}
for(i=0;body[i]!=','&&body[i]!=0;i++){
nextstep[i]=body[i];
}
nextstep[i]=0;
printf("nextstep %s %d\n",nextstep,i);
}
else if(!strcmp(in,"continue")||!strcmp(in,"c")){
//continue the program
lastcommand=2;
}
else if(!strcmp(in,"quit")||!strcmp(in,"q")){
//quit the program
takeinput=0;
}
//insert more commands here possibly break, print, set, etc
else if(!strcmp(in,"print")||!strcmp(in,"p")){
frame * curframe=cstack::thiscstack.getframe(cstack::thiscstack.stacksize-1);
scanf("%s",in);
int ret=cstack::thiscstack.find(curframe,in);
if(ret>=0){
stack *s = curframe->sstack[ret];
viz(s);
}
else
printf("variable does not exist %s\n",in);
}
}
return;
}
void SecondOrderTrustRegion::doOptimize(OptimizationProblemSecond& problem) {
#ifdef NICE_USELIB_LINAL
bool previousStepSuccessful = true;
double previousError = problem.objective();
problem.computeGradientAndHessian();
double delta = computeInitialDelta(problem.gradientCached(), problem.hessianCached());
double normOldPosition = 0.0;
// iterate
for (int iteration = 0; iteration < maxIterations; iteration++) {
// Log::debug() << "iteration, objective: " << iteration << ", "
// << problem.objective() << std::endl;
if (previousStepSuccessful && iteration > 0) {
problem.computeGradientAndHessian();
}
// gradient-norm stopping condition
if (problem.gradientNormCached() < epsilonG) {
Log::debug() << "SecondOrderTrustRegion stopped: gradientNorm "
<< iteration << std::endl;
break;
}
LinAlVector gradient(problem.gradientCached().linalCol());
LinAlVector negativeGradient(gradient);
negativeGradient.multiply(-1.0);
double lambda;
int lambdaMinIndex = -1;
// FIXME will this copy the matrix? no copy needed here!
LinAlMatrix hessian(problem.hessianCached().linal());
LinAlMatrix l(hessian);
try {
//l.CHdecompose(); // FIXME
choleskyDecompose(hessian, l);
lambda = 0.0;
} catch (...) { //(LinAl::BLException& e) { // FIXME
const LinAlVector& eigenValuesHessian = LinAl::eigensym(hessian);
// find smallest eigenvalue
lambda = std::numeric_limits<double>::infinity();
for (unsigned int i = 0; i < problem.dimension(); i++) {
const double eigenValue = eigenValuesHessian(i);
if (eigenValue < lambda) {
lambda = eigenValue;
lambdaMinIndex = i;
}
}
const double originalLambda = lambda;
lambda = -lambda * (1.0 + epsilonLambda);
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
try {
//l.CHdecompose(); // FIXME
LinAlMatrix temp(l);
choleskyDecompose(temp, l);
} catch (...) { // LinAl::BLException& e) { // FIXME
/*
* Cholesky factorization failed, which should theortically not be
* possible (can still happen due to numeric problems,
* also note that there seems to be a bug in CHdecompose()).
* Try a really great lambda as last attempt.
*/
// lambda = -originalLambda * (1.0 + epsilonLambda * 100.0)
// + 2.0 * epsilonM;
lambda = fabs(originalLambda) * (1.0 + epsilonLambda * 1E5)
+ 1E3 * epsilonM;
// lambda = fabs(originalLambda);// * 15.0;
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
try {
//l.CHdecompose(); // FIXME
LinAlMatrix temp(l);
choleskyDecompose(temp, l);
} catch (...) { // (LinAl::BLException& e) { // FIXME
// Cholesky factorization failed again, give up.
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
const LinAlVector& eigenValuesL = LinAl::eigensym(l);
Log::detail()
<< "l.CHdecompose: exception" << std::endl
//<< e.what() << std::endl // FIXME
<< "lambda=" << lambda << std::endl
<< "l" << std::endl << l
<< "hessian" << std::endl << hessian
<< "gradient" << std::endl << gradient
<< "eigenvalues hessian" << std::endl << eigenValuesHessian
<< "eigenvalues l" << std::endl << eigenValuesL
<< std::endl;
return;
}
}
}
// FIXME will the copy the vector? copy is needed here
LinAlVector step(negativeGradient);
l.CHsubstitute(step);
double normStepSquared = normSquared(step);
double normStep = sqrt(normStepSquared);
// exact: if normStep <= delta
if (normStep - delta <= tolerance) {
// exact: if lambda == 0 || normStep == delta
if (std::fabs(lambda) < tolerance
|| std::fabs(normStep - delta) < tolerance) {
// done
} else {
LinAlMatrix eigenVectors(problem.dimension(), problem.dimension());
eigensym(hessian, eigenVectors);
double a = 0.0;
double b = 0.0;
double c = 0.0;
for (unsigned int i = 0; i < problem.dimension(); i++) {
const double ui = eigenVectors(i, lambdaMinIndex);
const double si = step(i);
a += ui * ui;
b += si * ui;
c += si * si;
}
b *= 2.0;
c -= delta * delta;
const double sq = sqrt(b * b - 4.0 * a * c);
const double root1 = 0.5 * (-b + sq) / a;
const double root2 = 0.5 * (-b - sq) / a;
LinAlVector step1(step);
LinAlVector step2(step);
for (unsigned int i = 0; i < problem.dimension(); i++) {
step1(i) += root1 * eigenVectors(i, lambdaMinIndex);
step2(i) += root2 * eigenVectors(i, lambdaMinIndex);
}
const double psi1
= dotProduct(gradient, step1)
+ 0.5 * productVMV(step1, hessian, step1);
const double psi2
= dotProduct(gradient, step2)
+ 0.5 * productVMV(step2, hessian, step2);
if (psi1 < psi2) {
step = step1;
} else {
step = step2;
}
}
} else {
for (unsigned int subIteration = 0;
subIteration < maxSubIterations;
subIteration++) {
if (std::fabs(normStep - delta) <= kappa * delta) {
break;
}
// NOTE specialized algorithm may be more effifient than solvelineq
// (l is lower triangle!)
// Only lower triangle values of l are valid (other implicitly = 0.0),
// but solvelineq doesn't know that -> explicitly set to 0.0
for (int i = 0; i < l.rows(); i++) {
for (int j = i + 1; j < l.cols(); j++) {
l(i, j) = 0.0;
}
}
LinAlVector y(step.size());
try {
y = solvelineq(l, step);
} catch (LinAl::Exception& e) {
// FIXME if we end up here, something is pretty wrong!
// give up the whole thing
Log::debug() << "SecondOrderTrustRegion stopped: solvelineq failed "
<< iteration << std::endl;
return;
}
lambda += (normStep - delta) / delta
* normStepSquared / normSquared(y);
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
try {
//l.CHdecompose(); // FIXME
LinAlMatrix temp(l);
choleskyDecompose(temp, l);
} catch (...) { // (LinAl::BLException& e) { // FIXME
Log::detail()
<< "l.CHdecompose: exception" << std::endl
// << e.what() << std::endl // FIXME
<< "lambda=" << lambda << std::endl
<< "l" << std::endl << l
<< std::endl;
return;
}
step = negativeGradient;
l.CHsubstitute(step);
normStepSquared = normSquared(step);
normStep = sqrt(normStepSquared);
}
}
// computation of step is complete, convert to NICE::Vector
Vector stepLimun(step);
// minimal change stopping condition
if (changeIsMinimal(stepLimun, problem.position())) {
Log::debug() << "SecondOrderTrustRegion stopped: change is minimal "
<< iteration << std::endl;
break;
}
if (previousStepSuccessful) {
normOldPosition = problem.position().normL2();
}
// set new region parameters (to be verified later)
problem.applyStep(stepLimun);
// compute reduction rate
const double newError = problem.objective();
//Log::debug() << "newError: " << newError << std::endl;
const double errorReduction = newError - previousError;
const double psi = problem.gradientCached().scalarProduct(stepLimun)
+ 0.5 * productVMV(step, hessian, step);
double rho;
if (std::fabs(psi) <= epsilonRho
&& std::fabs(errorReduction) <= epsilonRho) {
rho = 1.0;
} else {
rho = errorReduction / psi;
}
// NOTE psi and errorReduction checks added to the algorithm
// described in Ferid Bajramovic's Diplomarbeit
if (rho < eta1 || psi >= 0.0 || errorReduction > 0.0) {
previousStepSuccessful = false;
problem.unapplyStep(stepLimun);
delta = alpha2 * normStep;
} else {
previousStepSuccessful = true;
previousError = newError;
if (rho >= eta2) {
const double newDelta = alpha1 * normStep;
if (newDelta > delta) {
delta = newDelta;
}
} // else: don't change delta
}
// delta stopping condition
if (delta < epsilonDelta * normOldPosition) {
Log::debug() << "SecondOrderTrustRegion stopped: delta too small "
<< iteration << std::endl;
break;
}
}
#else // no linal
fthrow(Exception,
"SecondOrderTrustRegion needs LinAl. Please recompile with LinAl");
#endif
}
dgInt32 dgCollisionCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count = 0;
if (normal.m_x < dgFloat32 (-0.995f)) {
if (normal.m_x < dgFloat32(-0.9995f)) {
dgMatrix matrix(normal);
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radio0);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i].CompProduct4(scale)) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else {
dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f));
dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f));
dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut);
if (count > 6) {
dgInt32 dy = 2 * 6;
dgInt32 dx = 2 * count;
dgInt32 acc = dy - count;
dgInt32 index = 0;
for (dgInt32 i = 0; i < count; i++) {
if (acc > 0) {
contactsOut[index] = contactsOut[i];
index++;
acc -= dx;
}
acc += dy;
}
count = index;
}
for (dgInt32 i = 0; i < count; i++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
}
} else if (normal.m_x > dgFloat32 (0.995f)) {
if (normal.m_x > dgFloat32 (0.9995f)) {
dgMatrix matrix(normal);
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radio1);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i].CompProduct4(scale)) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else {
dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f));
dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f));
dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut);
if (count > 6) {
dgInt32 dy = 2 * 6;
dgInt32 dx = 2 * count;
dgInt32 acc = dy - count;
dgInt32 index = 0;
for (dgInt32 i = 0; i < count; i++) {
if (acc > 0) {
contactsOut[index] = contactsOut[i];
index++;
acc -= dx;
}
acc += dy;
}
count = index;
}
for (dgInt32 i = 0; i < count; i++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
}
} else {
dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f));
dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f));
dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
count = 0;
int i0 = 3;
dgVector test0((m_profile[i0] - origin1).DotProduct4(normal1));
for (int i = 0; (i < 4) && (count < 2); i++) {
dgVector test1((m_profile[i] - origin1).DotProduct4(normal1));
dgVector acrossPlane(test0.CompProduct4(test1));
if (acrossPlane.m_x < 0.0f) {
dgVector step(m_profile[i] - m_profile[i0]);
contactsOut[count] = m_profile[i0] - step.Scale4(test0.m_x / (step.DotProduct4(normal1).m_x));
count++;
}
i0 = i;
test0 = test1;
}
for (dgInt32 i = 0; i < count; i++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
}
return count;
}
/*!
Set the minimum value of the range
\param value Minimum value
\sa setMaxValue(), minVal()
*/
void QwtCounter::setMinValue( double value )
{
setRange( value, maxValue(), step() );
}
Iterator(V *start, V *end) : slot(start), end(end), current(T::invalid_value())
{
step();
}
void ncr5390_device::device_timer(emu_timer &timer, device_timer_id id, int param, void *ptr)
{
step(true);
}
V operator ++()
{
step();
return current;
}
void ncr5390_device::recv_byte()
{
scsi_bus->ctrl_wait(scsi_refid, S_REQ, S_REQ);
state = (state & STATE_MASK) | (RECV_WAIT_REQ_1 << SUB_SHIFT);
step(false);
}
V operator ++(int)
{
V result = current;
step();
return result;
}
void Dayin_Fun(u8 dayin_par)
{
u8 datatime_str[6];
u8 drive_illegalstr[666];
u32 current_u32time=0; // 当前的时间
u32 old2day_u32time=0; // 前2个日历天的时间 current-2x86400 (172800)
u32 read_u32time=0;
u32 read_u32_ENDTime=0; // 读取记录中结束时间
u8 i=0,efftive_counter=0,check_limit=0;// check_limit 表示需要检索记录的数目
u8 print_buf[70];
u8 leftminute=0; // 当前分钟数值
u8 find_chaoshi_record=0; //
if(dayin_par==1)
{
memcpy(dayin_chepaihaoma+17,Vechicle_Info.Vech_Num,8); // 2
memcpy(dayin_chepaifenlei+17,Vechicle_Info.Vech_Type,strlen(Vechicle_Info.Vech_Type)); // 3
memcpy(dayin_cheliangVIN+10,Vechicle_Info.Vech_VIN,17); // 4
memcpy(dayin_driver_NUM+13,JT808Conf_struct.Driver_Info.DriveName,21); //5
memcpy(dayin_driver_card+24,Read_ICinfo_Reg.DriverCard_ID,18);//6
memcpy((char *)dayin_data_time+11,(char *)Dis_date,20); //7
switch(DaYin)
{
case 1:
if(step(50,1000))
{
Menu_txt_state=1;
pMenuItem=&Menu_TXT;
pMenuItem->show();
pMenuItem->timetick( 10 );
pMenuItem->keypress( 10 );
//-------------------------
DaYin=0;
print_rec_flag=0;
GPIO_ResetBits(GPIOB,GPIO_Pin_7);//打印关电
//-----------------------------------------------------
print_workingFlag=0; // 打印状态进行中
//-----------------------------------------------
}
else
{
Menu_txt_state=5;
pMenuItem=&Menu_TXT;
pMenuItem->show();
pMenuItem->timetick( 10 );
pMenuItem->keypress( 10 );
//--------------------------------------------
DaYin++;
GPIO_SetBits( GPIOB, GPIO_Pin_7 );
}
break;
case 2://车牌号码 9
printer((const char *)dayin_chepaihaoma);
printer((const char *)dayin_chepaifenlei);
printer((const char *)dayin_cheliangVIN);
printer((const char *)dayin_driver_card);
if((VdrData.H_15[0]==0x02)&&(Limit_max_SateFlag==0))
printer("\r\n速度状态: 异常");
else
printer("\r\n速度状态: 正常");
printer((const char *)dayin_data_time);//00/00/00 00:00:00
printer("2个日历天内超时驾驶记录:\r\n");
time_now=Get_RTC(); // RTC 相关
Time2BCD(datatime_str);
current_u32time=Time_sec_u32(datatime_str,6);
old2day_u32time=current_u32time-172800; // 2个日历天内的时间
if(Vdr_Wr_Rd_Offset.V_11H_full)
check_limit=VDR_11_MAXindex;
else
check_limit=Vdr_Wr_Rd_Offset.V_11H_Write;
if(check_limit)
{
for(i=0;i<check_limit;i++)
{
memset(drive_illegalstr,0,sizeof(drive_illegalstr));
if(get_11h(i, drive_illegalstr)==0) //50 packetsize num=100
continue;
read_u32time=Time_sec_u32(drive_illegalstr+18,6); // 连续驾驶起始时间
read_u32_ENDTime=Time_sec_u32(drive_illegalstr+24,6); // 连续驾驶结束时间
if(read_u32time>=old2day_u32time)
{
if((read_u32_ENDTime>read_u32time)&&((read_u32_ENDTime-read_u32time)>(4*60*60)))
{ // 结束时间大于起始时间，且差值大于4个小时
efftive_counter++;
memset(print_buf,0,sizeof(print_buf));
sprintf(print_buf,"\r\n记录 %d:",efftive_counter);
printer(print_buf);
memcpy(dayin_driver_card+24,drive_illegalstr,18);//6
printer((const char *)dayin_driver_card);
memset(print_buf,0,sizeof(print_buf));
sprintf(print_buf,"\r\n 连续驾驶开始时间: \r\n 20%2d-%d%d-%d%d %d%d:%d%d:%d%d",BCD2HEX(drive_illegalstr[18]),\
BCD2HEX(drive_illegalstr[19])/10,BCD2HEX(drive_illegalstr[19])%10,BCD2HEX(drive_illegalstr[20])/10,BCD2HEX(drive_illegalstr[20])%10,\
BCD2HEX(drive_illegalstr[21])/10,BCD2HEX(drive_illegalstr[21])%10,BCD2HEX(drive_illegalstr[22])/10,BCD2HEX(drive_illegalstr[22])%10,\
BCD2HEX(drive_illegalstr[23])/10,BCD2HEX(drive_illegalstr[23])%10);
printer(print_buf);
memset(print_buf,0,sizeof(print_buf));
sprintf(print_buf,"\r\n 连续驾驶结束时间: \r\n 20%2d-%d%d-%d%d %d%d:%d%d:%d%d",BCD2HEX(drive_illegalstr[24]),\
BCD2HEX(drive_illegalstr[25])/10,BCD2HEX(drive_illegalstr[25])%10,BCD2HEX(drive_illegalstr[26])/10,BCD2HEX(drive_illegalstr[26])%10,\
BCD2HEX(drive_illegalstr[27])/10,BCD2HEX(drive_illegalstr[27])%10,BCD2HEX(drive_illegalstr[28])/10,BCD2HEX(drive_illegalstr[28])%10,\
BCD2HEX(drive_illegalstr[29])/10,BCD2HEX(drive_illegalstr[29])%10);
printer(print_buf);
find_chaoshi_record=enable; // find record
}
}
}
if(find_chaoshi_record==0)
printer("\r\n无超时驾驶记录\r\n");
}
else
printer("\r\n无超时驾驶记录\r\n");
// 最近15分钟 平均速度
printer("\r\n停车前15分钟每分钟平均速度:");
memset(drive_illegalstr,0,sizeof(drive_illegalstr));
leftminute=Api_avrg15minSpd_Content_read(drive_illegalstr);
if(leftminute==0)
printer("\r\n 暂无最近15分钟停车记录");
else
if(leftminute==105)
{
for(i=0;i<15;i++)
{
memset(print_buf,0,sizeof(print_buf));
sprintf(print_buf,"\r\n 20%2d-%d%d-%d%d %d%d:%d%d %d km/h",drive_illegalstr[i*7],\
drive_illegalstr[i*7+1]/10,drive_illegalstr[i*7+1]%10,drive_illegalstr[i*7+2]/10,drive_illegalstr[i*7+2]%10,\
drive_illegalstr[i*7+3]/10,drive_illegalstr[i*7+3]%10,drive_illegalstr[i*7+4]/10,drive_illegalstr[i*7+4]%10,\
drive_illegalstr[i*7+6]);
printer(print_buf);
}
}
printer("\r\n \r\n");
printer("\r\n\r\n 签名 : ______________\r\n");
DaYin++;
break;
case 3:
step(50,1000);
DaYin++;
break;
case 4:
step(50,1000);
DaYin++;
break;
case 5:
DaYin=0;
print_rec_flag=0;
GPIO_ResetBits( GPIOB, GPIO_Pin_7 );
print_workingFlag=0; // 打印状态进行中
pMenuItem=&Menu_1_Idle;
pMenuItem->show();
break;
}
}
}
int main(void)
{
//LOCALS
unsigned int temp;
unsigned int channel1, channel2;
unsigned int M1_stepPeriod, M2_stepPeriod, M3_stepPeriod, M4_stepPeriod;
M1_stepPeriod = M2_stepPeriod = M3_stepPeriod = M4_stepPeriod = 1000; // in tens of u-seconds
unsigned char M1_state = 0, M2_state = 0, M3_state = 0, M4_state = 0;
unsigned int step_counter;
SYSTEMConfig(GetSystemClock(), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
/* TIMER1 - now configured to interrupt at 10 khz (every 100us) */
OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, T1_TICK);
ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2);
/* TIMER2 - 100 khz interrupt for distance measure*/
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, T2_TICK);
ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_3); //It is off until trigger
/* PORTA b2 and b3 for servo-PWM */
mPORTAClearBits(BIT_2 | BIT_3);
mPORTASetPinsDigitalOut(BIT_2 | BIT_3);
/* ULTRASONICS: some bits of PORTB for ultrasonic sensors */
PORTResetPins(IOPORT_B, BIT_8 | BIT_9| BIT_10 | BIT_11 );
PORTSetPinsDigitalOut(IOPORT_B, BIT_8 | BIT_9| BIT_10 | BIT_11); //trigger
/* Input Capture pins for echo signals */
//interrupt on every risging/falling edge starting with a rising edge
PORTSetPinsDigitalIn(IOPORT_D, BIT_8| BIT_9| BIT_10| BIT_11); //INC1, INC2, INC3, INC4 Pin
mIC1ClearIntFlag();
OpenCapture1( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//front
ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
OpenCapture2( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//back
ConfigIntCapture2(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
OpenCapture3( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//left
ConfigIntCapture3(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
OpenCapture4( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//right
ConfigIntCapture4(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
/* PINS used for the START (RD13) BUTTON */
PORTSetPinsDigitalIn(IOPORT_D, BIT_13);
#define CONFIG (CN_ON | CN_IDLE_CON)
#define INTERRUPT (CHANGE_INT_ON | CHANGE_INT_PRI_2)
mCNOpen(CONFIG, CN19_ENABLE, CN19_PULLUP_ENABLE);
temp = mPORTDRead();
/* PORT D and E for motors */
//motor 1
mPORTDSetBits(BIT_4 | BIT_5 | BIT_6 | BIT_7); // Turn on PORTD on startup.
mPORTDSetPinsDigitalOut(BIT_4 | BIT_5 | BIT_6 | BIT_7); // Make PORTD output.
//motor 2
mPORTCSetBits(BIT_1 | BIT_2 | BIT_3 | BIT_4); // Turn on PORTC on startup.
mPORTCSetPinsDigitalOut(BIT_1 | BIT_2 | BIT_3 | BIT_4); // Make PORTC output.
//motor 3 and 4
mPORTESetBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7); // Turn on PORTE on startup.
mPORTESetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7); // Make PORTE output.
// UART2 to connect to the PC.
// This initialization assumes 36MHz Fpb clock. If it changes,
// you will have to modify baud rate initializer.
UARTConfigure(UART2, UART_ENABLE_PINS_TX_RX_ONLY);
UARTSetFifoMode(UART2, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
UARTSetLineControl(UART2, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
UARTSetDataRate(UART2, GetPeripheralClock(), BAUD);
UARTEnable(UART2, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));
// Configure UART2 RX Interrupt
INTEnable(INT_SOURCE_UART_RX(UART2), INT_ENABLED);
INTSetVectorPriority(INT_VECTOR_UART(UART2), INT_PRIORITY_LEVEL_2);
INTSetVectorSubPriority(INT_VECTOR_UART(UART2), INT_SUB_PRIORITY_LEVEL_0);
/* PORTD for LEDs - DEBUGGING */
mPORTDClearBits(BIT_0 | BIT_1 | BIT_2);
mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2);
// Congifure Change/Notice Interrupt Flag
ConfigIntCN(INTERRUPT);
// configure for multi-vectored mode
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
// enable interrupts
INTEnableInterrupts();
counterDistanceMeasure=600; //measure ULTRASONICS distance each 60 ms
while (1) {
/*
//Process UART command
switch (COMMAND) {
case 'q':
if (MotorsON)
MotorsON = 0;
else
MotorsON = 1;
break;
case 'w':
M1forward = M2forward = 1;
break;
case 's':
M1forward = M2forward = 0;
break;
case 'a': //left
M1forward = 0;
M2forward = 1;
break;
case 'd': //right
M1forward = 1;
M2forward = 0;
break;
case 0:
default:
break;
}
COMMAND = 0;
*/
/***************** Robot MAIN state machine *****************/
switch (Robo_State) {
case 0:
MotorsON = 0;
Robo_State = 0;
step_counter = 0;
break;
case 1: //forward
/*
if (step_counter == 0) {
step_counter = 50 * SPC_front; //1 cm es aprox. = 11.3636 pasos
MotorsON = 1;
M1forward = M2forward = M3forward = M4forward= 1;
Robo_State = 2;
}
*/
go ('F');
MotorsON = 1;
Robo_State = 2;
break;
case 2:
if (frontDistance < 20) {
go('B');
}
if (backDistance < 20) {
go('F');
}
if (rightDistance < 20) {
go('L');
}
if (leftDistance < 20) {
go('R');
}
break;
default:
if (step_counter == 0) {
Robo_State = 0;
}
break;
}
if (frontDistance < 20 || backDistance < 20 || leftDistance < 20 || rightDistance < 20)
mPORTDSetBits(BIT_0);
else
mPORTDClearBits(BIT_0);
/***************************************************************/
M1_stepPeriod = 50; // value between 50 and 100 in tens of u-seconds (step period)
M2_stepPeriod = 50;
M3_stepPeriod = 50; // influences the speed of the wheels
M4_stepPeriod = 50;
if (MotorsON) {
/****************************
MOTOR MAP
M1 O-------------O M2 ON EVEN MOTORS, STEPS MUST BE INVERTE
| /\ | i.e. FORWARD IS BACKWARD
| / \ |
| || |
| || |
M3 O-------------O M4
*****************************/
if (M1_counter == 0) {
switch (M1_state) {
case 0: // set 0011
step (0x3 , 1);
if (M1forward)
M1_state = 1;
else
M1_state = 3;
break;
case 1: // set 1001
step (0x9 , 1);
if (M1forward)
M1_state = 2;
else
M1_state = 0;
break;
case 2: // set 1100
step (0xC , 1);
if (M1forward)
M1_state = 3;
else
M1_state = 1;
break;
case 3: // set 0110
default:
step (0x6 , 1);
if (M1forward)
M1_state = 0;
else
M1_state = 2;
break;
}
M1_counter = M1_stepPeriod;
step_counter--;
}
if (M2_counter == 0) {
switch (M2_state) {
case 0: // set 0011
step (0x3 , 2);
if (M2forward)
M2_state = 1;
else
M2_state = 3;
break;
case 1: // set 0110
step (0x6 , 2);
if (M2forward)
M2_state = 2;
else
M2_state = 0;
break;
case 2: // set 1100
step (0xC , 2);
if (M2forward)
M2_state = 3;
else
M2_state = 1;
break;
case 3: // set 1001
default:
step (0x9 , 2);
if (M2forward)
M2_state = 0;
else
M2_state = 2;
break;
}
M2_counter = M2_stepPeriod;
}
if (M3_counter == 0) {
switch (M3_state) {
case 0: // set 0011
step (0x3 , 3);
if (M3forward)
M3_state = 1;
else
M3_state = 3;
break;
case 1: // set 1001
step (0x9 , 3);
if (M3forward)
M3_state = 2;
else
M3_state = 0;
break;
case 2: // set 1100
step (0xC , 3);
if (M3forward)
M3_state = 3;
else
M3_state = 1;
break;
case 3: // set 0110
default:
step (0x6 , 3);
if (M3forward)
M3_state = 0;
else
M3_state = 2;
break;
}
M3_counter = M3_stepPeriod;
}
if (M4_counter == 0) {
switch (M4_state) {
case 0: // set 0011
step (0x3 , 4);
if (M4forward)
M4_state = 1;
else
M4_state = 3;
break;
case 1: // set 0110
step (0x6 , 4);
if (M4forward)
M4_state = 2;
else
M4_state = 0;
break;
case 2: // set 1100
step (0xC , 4);
if (M4forward)
M4_state = 3;
else
M4_state = 1;
break;
case 3: // set 1001
default:
step (0x9 , 4);
if (M4forward)
M4_state = 0;
else
M4_state = 2;
break;
}
M4_counter = M4_stepPeriod;
}
if (step_counter == 0)
MotorsON = 0;
} else {
//motors off
mPORTDSetBits(BIT_4 | BIT_5 | BIT_6 | BIT_7);
mPORTCSetBits(BIT_1 | BIT_2 | BIT_3 | BIT_4);
mPORTESetBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7);
}
/******* TEST CODE, toggles the servos (from 90 deg. to -90 deg.) every 1 s. ********/
if (auxcounter == 0) {
if (servo1_angle == 90)
servo1_angle = -90;
else
servo1_angle = 90;
if (servo2_angle == 90)
servo2_angle = -90;
else
servo2_angle = 90;
auxcounter = 10000; // toggle angle every 1 s.
}
/***********************************************************************************/
/******* SERVO CONTROL ********/
/*
Changing the global servoX_angle at any point in the code will
move the servo to the desired angle.
*/
servo1_counter = (servo1_angle + 90)*(18)/180 + 6; // between 600 and 2400 us
if (servo1_period == 0) {
mPORTASetBits(BIT_2);
servo1_period = SERVOMAXPERIOD; /* 200 * 100us = 20000us period */
}
servo2_counter = (servo2_angle + 90)*(18)/180 + 6; // between 600 and 2400 us
if (servo2_period == 0) {
mPORTASetBits(BIT_3);
servo2_period = SERVOMAXPERIOD; /* 200 * 100us = 20000us period */
}
/*******************************/
} /* end of while(1) */
return 0;
}
int main(int argc, char *argv[]) {
Teuchos::GlobalMPISession mpiSession(&argc, &argv);
// This little trick lets us print to std::cout only if a (dummy) command-line argument is provided.
int iprint = argc - 1;
Teuchos::RCP<std::ostream> outStream;
Teuchos::oblackholestream bhs; // outputs nothing
if (iprint > 0)
outStream = Teuchos::rcp(&std::cout, false);
else
outStream = Teuchos::rcp(&bhs, false);
int errorFlag = 0;
// *** Example body.
try {
Teuchos::RCP<ROL::Objective<RealT> > obj;
Teuchos::RCP<ROL::EqualityConstraint<RealT> > constr;
Teuchos::RCP<std::vector<RealT> > x_rcp = Teuchos::rcp( new std::vector<RealT> (0, 0.0) );
Teuchos::RCP<std::vector<RealT> > sol_rcp = Teuchos::rcp( new std::vector<RealT> (0, 0.0) );
ROL::StdVector<RealT> x(x_rcp); // Iteration vector.
ROL::StdVector<RealT> sol(sol_rcp); // Reference solution vector.
// Retrieve objective, constraint, iteration vector, solution vector.
ROL::ZOO::getSimpleEqConstrained <RealT, ROL::StdVector<RealT>, ROL::StdVector<RealT>, ROL::StdVector<RealT>, ROL::StdVector<RealT> > (obj, constr, x, sol);
Teuchos::ParameterList parlist;
// Define Step
parlist.set("Nominal SQP Optimality Solver Tolerance", 1.e-2);
ROL::CompositeStepSQP<RealT> step(parlist);
// Run derivative checks, etc.
int dim = 5;
int nc = 3;
RealT left = -1e0, right = 1e0;
Teuchos::RCP<std::vector<RealT> > xtest_rcp = Teuchos::rcp( new std::vector<RealT> (dim, 0.0) );
Teuchos::RCP<std::vector<RealT> > g_rcp = Teuchos::rcp( new std::vector<RealT> (dim, 0.0) );
Teuchos::RCP<std::vector<RealT> > d_rcp = Teuchos::rcp( new std::vector<RealT> (dim, 0.0) );
Teuchos::RCP<std::vector<RealT> > v_rcp = Teuchos::rcp( new std::vector<RealT> (dim, 0.0) );
Teuchos::RCP<std::vector<RealT> > vc_rcp = Teuchos::rcp( new std::vector<RealT> (nc, 0.0) );
Teuchos::RCP<std::vector<RealT> > vl_rcp = Teuchos::rcp( new std::vector<RealT> (nc, 0.0) );
ROL::StdVector<RealT> xtest(xtest_rcp);
ROL::StdVector<RealT> g(g_rcp);
ROL::StdVector<RealT> d(d_rcp);
ROL::StdVector<RealT> v(v_rcp);
ROL::StdVector<RealT> vc(vc_rcp);
ROL::StdVector<RealT> vl(vl_rcp);
// set xtest, d, v
for (int i=0; i<dim; i++) {
(*xtest_rcp)[i] = ( (RealT)rand() / (RealT)RAND_MAX ) * (right - left) + left;
(*d_rcp)[i] = ( (RealT)rand() / (RealT)RAND_MAX ) * (right - left) + left;
(*v_rcp)[i] = ( (RealT)rand() / (RealT)RAND_MAX ) * (right - left) + left;
}
// set vc, vl
for (int i=0; i<nc; i++) {
(*vc_rcp)[i] = ( (RealT)rand() / (RealT)RAND_MAX ) * (right - left) + left;
(*vl_rcp)[i] = ( (RealT)rand() / (RealT)RAND_MAX ) * (right - left) + left;
}
obj->checkGradient(xtest, d, true, *outStream); *outStream << "\n";
obj->checkHessVec(xtest, v, true, *outStream); *outStream << "\n";
obj->checkHessSym(xtest, d, v, true, *outStream); *outStream << "\n";
constr->checkApplyJacobian(xtest, v, vc, true, *outStream); *outStream << "\n";
constr->checkApplyAdjointJacobian(xtest, vl, vc, xtest, true, *outStream); *outStream << "\n";
constr->checkApplyAdjointHessian(xtest, vl, d, xtest, true, *outStream); *outStream << "\n";
Teuchos::RCP<std::vector<RealT> > v1_rcp = Teuchos::rcp( new std::vector<RealT> (dim, 0.0) );
Teuchos::RCP<std::vector<RealT> > v2_rcp = Teuchos::rcp( new std::vector<RealT> (nc, 0.0) );
ROL::StdVector<RealT> v1(v1_rcp);
ROL::StdVector<RealT> v2(v2_rcp);
RealT augtol = 1e-8;
constr->solveAugmentedSystem(v1, v2, d, vc, xtest, augtol);
// Define Status Test
RealT gtol = 1e-12; // norm of gradient tolerance
RealT ctol = 1e-12; // norm of constraint tolerance
RealT stol = 1e-18; // norm of step tolerance
int maxit = 1000; // maximum number of iterations
ROL::StatusTestSQP<RealT> status(gtol, ctol, stol, maxit);
// Define Algorithm
ROL::DefaultAlgorithm<RealT> algo(step, status, false);
// Run Algorithm
vl.zero();
//(*x_rcp)[0] = 3.0; (*x_rcp)[1] = 2.0; (*x_rcp)[2] = 2.0; (*x_rcp)[3] = 1.0; (*x_rcp)[4] = 1.0;
//(*x_rcp)[0] = -5.0; (*x_rcp)[1] = -5.0; (*x_rcp)[2] = -5.0; (*x_rcp)[3] = -6.0; (*x_rcp)[4] = -6.0;
std::vector<std::string> output = algo.run(x, g, vl, vc, *obj, *constr, false);
for ( unsigned i = 0; i < output.size(); i++ ) {
*outStream << output[i];
}
// Compute Error
*outStream << "\nReference solution x_r =\n";
*outStream << std::scientific << " " << (*sol_rcp)[0] << "\n";
*outStream << std::scientific << " " << (*sol_rcp)[1] << "\n";
*outStream << std::scientific << " " << (*sol_rcp)[2] << "\n";
*outStream << std::scientific << " " << (*sol_rcp)[3] << "\n";
*outStream << std::scientific << " " << (*sol_rcp)[4] << "\n";
*outStream << "\nOptimal solution x =\n";
*outStream << std::scientific << " " << (*x_rcp)[0] << "\n";
*outStream << std::scientific << " " << (*x_rcp)[1] << "\n";
*outStream << std::scientific << " " << (*x_rcp)[2] << "\n";
*outStream << std::scientific << " " << (*x_rcp)[3] << "\n";
*outStream << std::scientific << " " << (*x_rcp)[4] << "\n";
x.axpy(-1.0, sol);
RealT abserr = x.norm();
RealT relerr = abserr/sol.norm();
*outStream << std::scientific << "\n Absolute Error: " << abserr;
*outStream << std::scientific << "\n Relative Error: " << relerr << "\n";
if ( relerr > sqrt(ROL::ROL_EPSILON) ) {
errorFlag += 1;
}
}
catch (std::logic_error err) {
*outStream << err.what() << "\n";
errorFlag = -1000;
}; // end try
if (errorFlag != 0)
std::cout << "End Result: TEST FAILED\n";
else
std::cout << "End Result: TEST PASSED\n";
return 0;
}
void MyRobot::run()
{
//initialitation counter pixel
int sum = 0;
//initialitation values RGB
unsigned char green = 0, red = 0, blue = 0;
//initialitation porcentage white
double percentage_white = 0.0;
// Get size of images for forward camera
int image_width_f = _forward_camera->getWidth();
int image_height_f = _forward_camera->getHeight();
cout << "Size of forward camera image: " << image_width_f << ", " << image_height_f << endl;
while (step(_time_step) != -1) {
sum = 0;
// Get current image from forward camera
const unsigned char *image_f = _forward_camera->getImage();
// Count number of pixels that are white
// (here assumed to have pixel value > 245 out of 255 for all color components)
for (int x = 0; x < image_width_f; x++) {
for (int y = 0; y < image_height_f; y++) {
green = _forward_camera->imageGetGreen(image_f, image_width_f, x, y);
red = _forward_camera->imageGetRed(image_f, image_width_f, x, y);
blue = _forward_camera->imageGetBlue(image_f, image_width_f, x, y);
if ((green > 195) && (red > 195) && (blue > 195)) {
sum = sum + 1;
}
}
}
percentage_white = (sum / (float) (image_width_f * image_height_f)) * 100;
cout << "Percentage of white in forward camera image: " << percentage_white << endl;
// Control logic of the robot (FRONTAL ZONE)
if ((percentage_white >=80)) {
_mode = OBSTACLE_AVOID1;
cout << "NEAREST WALL." << endl;
}
else {
_mode = FORWARD;
cout << "MOVING FORWARD." << endl;
}
// Send actuators commands according to the mode
switch (_mode){
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case OBSTACLE_AVOID1: // MOVING BACK FACING LEFT
_left_speed = -MAX_SPEED / 3.0;
_right_speed = -MAX_SPEED / 20.0;
break;
default:
break;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
int
iSqrt(int value)
{
int root = 0;
step( 0);
step( 2);
step( 4);
step( 6);
step( 8);
step(10);
step(12);
step(14);
step(16);
step(18);
step(20);
step(22);
step(24);
step(26);
step(28);
step(30);
// round to the nearest integer, cuts max error in half
if(root < value) {
++root;
}
return root;
}
// Traditional ctor
Fl_Roller::Fl_Roller(int X,int Y,int W,int H,const char* L)
: Fl_Valuator(X,Y,W,H,L)
{
step(.001);
}
