BinaryWriter ElfFile::WriteDataSections( ElfContent& data)
{
// First write
BinaryWriter dataWritePos(dataSectionStart);
dataWritePos.Offset() = offsets.EndOfMapped();
for ( int i =0; i< header.Sections(); i++ ) {
Section& sec = *(data.sections[i]);
if ( sec.HasFileData()
&& !(sec.IsRelocTable() && sec.Address() ==0) )
{
if ( sec.IsNull() ) {
sec.DataStart() = 0;
SLOG_FROM (LOG_VERBOSE,
"ElfFile::WriteDataSections",
"Not Writing section (no data) : " << i)
} else {
if ( sec.Address() != 0 )
{
sec.DataStart() = offsets.AddressToOffset(sec.Address());
BinaryWriter writePos(dataSectionStart);
writePos.Offset() = sec.DataStart();
sec.WriteRawData(writePos);
SLOG_FROM (LOG_VERBOSE,
"ElfFile::WriteDataSections",
"Writing section (at address ) : " << i)
}
else
{
if ( sec.Alignment() != 0 ) {
RobotControl::RobotControl():
port("COM1"), is_time_synced(false),
is_recording(false), posTimer(this), moveTimer(this)
{
connect(&posTimer, SIGNAL(timeout()), this, SLOT(writePos()));
connect(&moveTimer, SIGNAL(timeout()), this, SLOT(moveTimeout()));
initPort();
}
// Public Methods //////////////////////////////////////////////////////////////
uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t sense, bool inv)
{
if (fingerIndex < MAX_FINGERS)
{
// configure pins
pinMode(dir0, OUTPUT); // set direction1 pin to output
pinMode(dir1, OUTPUT); // set direction2 pin to output
pinMode(sense, INPUT); // set sense pin to input
// attach all finger pins
_fingers[fingerIndex].Pin.dir[0] = dir0;
_fingers[fingerIndex].Pin.dir[1] = dir1;
_fingers[fingerIndex].Pin.sns = sense;
// enable the motor and disable finger inversion
_fingers[fingerIndex].invert = inv;
_fingers[fingerIndex].motorEn = true;
// set limits and initial values
setPosLimits(MIN_FINGER_POS,MAX_FINGER_POS);
setSpeedLimits(MIN_FINGER_SPEED,MAX_FINGER_SPEED);
writeSpeed(MAX_FINGER_SPEED);
writePos(MIN_FINGER_POS);
_fingers[fingerIndex].CurrDir = OPEN; // set dir to OPEN after initial writePos to configure finger dir
// if using the Atmega2560, set the PWM freq to > 20kHz to prevent humming
setPWMFreq(dir0, 0x01); // set PWM frequency to max freq
setPWMFreq(dir1, 0x01); // set PWM frequency to max freq
// initialise the timer
if(_timerSetupFlag == false)
{
_passMotorPtr(&fingerPosCtrl);
_timerSetup();
_timerSetupFlag = true;
}
_fingers[fingerIndex].Pin.isActive = true; // this must be set after the check
}
return fingerIndex;
}
void Motor::writePosDeg(double pos)
{
writePos(M_PI*pos/180);
}
NodeList* parse(char *string) {
NodeList *begin = new NodeList;
begin->next = begin->prev = null;
begin->node = null;
NodeList *nl = begin;
Sym *s = new Sym;
s->row = 1;
s->col = 0;
s->pos = 0;
s->string = string;
char c;
while ((c = getc(s))) {
if (isspace(c)) {
continue;
}
if (nl->node) {
nl->next = new NodeList;
nl->next->prev = nl;
nl = nl->next;
nl->next = NULL;
nl->node = NULL;
}
Node *n;
switch (c) {
case '+':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_PLUS;
nl->node = n;
writePos(n, s);
continue;
case '-':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_MINUS;
nl->node = n;
writePos(n, s);
continue;
case '*':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_MUL;
nl->node = n;
writePos(n, s);
continue;
case '/':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_DIV;
nl->node = n;
writePos(n, s);
continue;
case ',':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_COMMA;
nl->node = n;
writePos(n, s);
continue;
case '=':
n = new Node;
if (getc(s) == '=') {
n->value = new char[3];
n->value[0] = c;
n->value[1] = c;
n->value[2] = 0;
n->nodeType = NODE_TYPE_EQUAL;
}
else {
revokec(s);
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_ASSIGN;
}
nl->node = n;
writePos(n, s);
continue;
case '>':
n = new Node;
if (getc(s) == '=') {
n->value = new char[3];
n->value[0] = '>';
n->value[1] = '=';
n->value[2] = 0;
n->nodeType = NODE_TYPE_GREATER_EQ;
}
else {
revokec(s);
n->value = new char[2];
n->value[0] = '>';
n->value[1] = 0;
n->nodeType = NODE_TYPE_GREATER;
}
nl->node = n;
writePos(n, s);
continue;
case '!':
if (getc(s) == '=') {
n = new Node;
n->value = new char[3];
n->value[0] = '!';
n->value[1] = '=';
n->value[2] = 0;
n->nodeType = NODE_TYPE_NOT_EQUAL;
nl->node = n;
writePos(n, s);
}
else {
revokec(s);
lexerError(s, "Unknown construction, did you mean '!='");
}
continue;
case '<':
n = new Node;
if (getc(s) == '=') {
n->value = new char[3];
n->value[0] = '<';
n->value[1] = '=';
n->value[2] = 0;
n->nodeType = NODE_TYPE_LESS_EQ;
}
else {
revokec(s);
n->value = new char[2];
n->value[0] = '<';
n->value[1] = 0;
n->nodeType = NODE_TYPE_LESS;
}
nl->node = n;
writePos(n, s);
continue;
case ';':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_SEMICOLON;
nl->node = n;
writePos(n, s);
continue;
case '@':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_AMPERSAT;
nl->node = n;
writePos(n, s);
continue;
case '(':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_LPAREN;
nl->node = n;
writePos(n, s);
continue;
case ')':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_RPAREN;
nl->node = n;
writePos(n, s);
continue;
case '{':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_LBRACE;
nl->node = n;
writePos(n, s);
continue;
case '}':
n = new Node;
n->value = new char[2];
n->value[0] = c;
n->value[1] = 0;
n->nodeType = NODE_TYPE_RBRACE;
nl->node = n;
writePos(n, s);
continue;
case '#':
c = getc(s);
if (c == '*') {
do {
c = getc(s);
if (c == '*') {
c = getc(s);
if (c == '#') {
// comment is closed!
break;
}
}
} while (c);
}
else {
do {
c = getc(s);
if (c == '\n') {
break;
}
} while (c);
}
continue;
case '"':
int pos = s->pos + 1;
do {
c = getc(s);
if (c == '\n') {
lexerError(s, "Multi line strings are not supported.");
break;
}
else if (c == '"') {
// end of string
n = new Node;
int num = s->pos - pos;
n->value = (char*) malloc(sizeof(char) * num + 1);
strncpy(n->value, s->string + sizeof(char) * pos - 1, num);
n->nodeType = NODE_TYPE_STRING;
nl->node = n;
writePos(n, s);
break;
}
} while (c);
continue;
}
if (isdigit(c)) {
n = new Node();
n->nodeType = NODE_TYPE_NUMBER;
n->value = new char[1];
n->value[0] = 0;
do {
char* str = new char[2];
str[0] = c;
str[1] = 0;
strcat(n->value, str);
c = getc(s);
} while (isdigit(c));
if (c) revokec(s);
nl->node = n;
writePos(n, s);
}
else if (isalpha(c)) {
n = new Node();
n->value = new char[1];
n->value[0] = 0;
do {
char* str = new char[2];
str[0] = c;
str[1] = 0;
strcat(n->value, str);
c = getc(s);
} while (isalpha(c) || isdigit(c));
if (c) revokec(s);
nl->node = n;
if (strcmp(n->value, "def") == 0) {
n->nodeType = NODE_TYPE_DEF;
}
else if (strcmp(n->value, "print") == 0) {
n->nodeType = NODE_TYPE_PRINT;
}
else if (strcmp(n->value, "if") == 0) {
n->nodeType = NODE_TYPE_IF;
}
else if (strcmp(n->value, "then") == 0) {
n->nodeType = NODE_TYPE_THEN;
}
else if (strcmp(n->value, "else") == 0) {
n->nodeType = NODE_TYPE_ELSE;
}
else if (strcmp(n->value, "while") == 0) {
n->nodeType = NODE_TYPE_WHILE;
}
else if (strcmp(n->value, "break") == 0) {
n->nodeType = NODE_TYPE_BREAK;
}
else if (strcmp(n->value, "continue") == 0) {
n->nodeType = NODE_TYPE_CONTINUE;
}
else if (strcmp(n->value, "func") == 0) {
n->nodeType = NODE_TYPE_FUNC;
}
else {
n->nodeType = NODE_TYPE_IDENTIFIER;
}
writePos(n, s);
}
else if (c == EOF || c == 25) {
// rollback to previous node and break reading
nl = nl->prev;
break;
}
else {
lexerError(s, "Unexpected symbol.");
}
}
nl->next = new NodeList;
nl->next->node = new Node;
nl->next->prev = nl;
nl = nl->next;
nl->node->nodeType = NODE_TYPE_EOF;
nl->node->value = "eof";
writePos(nl->node, s);
return begin;
}
void Finger::close(void)
{
writePos(_fingers[fingerIndex].MaxPos);
}
void Finger::open(void)
{
writePos(_fingers[fingerIndex].MinPos);
}
void Finger::stopMotor(void)
{
// set target position to current pos
writePos(readPos());
}
