void TextLine::Add(TextSegment* segment) {
// simply skip rotated text
if (segment->Font()->Rotation() != 0.0) {
delete segment; return;
}
if (!Follows(segment)) {
Flush();
}
fSegments.AddItem(segment);
}
task taskControl()
{
Follows(main);
int ack;
char pfData[8];
SetSensorTouch(IN_4);
while(true) {
if(Sensor(IN_4) == 1)
{
StopAllTasks(); // stop the program
}
ack = bluetooth_receiveInt();
if (ack==1){
pfData[IR_LEFT]=-25;
pfData[IR_RIGHT]=25;
}
if (ack==2){
pfData[IR_LEFT]=25;
pfData[IR_RIGHT]=-25;
}
if (ack==3){
pfData[IR_LEFT]=0;
pfData[IR_RIGHT]=0;
}
if (ack==4){
pfData[IR_LEFT]=50;
pfData[IR_RIGHT]=50;
}
if (ack==5){
pfData[IR_LEFT]=-50;
pfData[IR_RIGHT]=-50;
}
if (ack==6){
pfData[IR_LEFT]=25;
pfData[IR_RIGHT]=50;
}
if (ack==7){
pfData[IR_LEFT]=50;
pfData[IR_RIGHT]=25;
}
if (ack==8){
pfData[IR_LEFT]=-25;
pfData[IR_RIGHT]=-50;
}
if (ack==9){
pfData[IR_LEFT]=-50;
pfData[IR_RIGHT]=-25;
}
motorControlDrive = (pfData[IR_LEFT] + pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
motorControlSteer = (pfData[IR_LEFT] - pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
Wait(CONTROL_WAIT);
}
}
//---------------------------------------------------------------------
// taskBalance
// This is the main balance task for the HTWay robot.
//
// Robot is assumed to start leaning on a wall. The first thing it
// does is take multiple samples of the gyro sensor to establish and
// initial gyro offset.
//
// After an initial gyro offset is established, the robot backs up
// against the wall until it falls forward, when it detects the
// forward fall, it start the balance loop.
//
// The main state variables are:
// gyroAngle This is the angle of the robot, it is the results of
// integrating on the gyro value.
// Units: degrees
// gyroSpeed The value from the Gyro Sensor after offset subtracted
// Units: degrees/second
// motorPos This is the motor position used for balancing.
// Note that this variable has two sources of input:
// Change in motor position based on the sum of
// MotorRotationCount of the two motors,
// and,
// forced movement based on user driving the robot.
// Units: degrees (sum of the two motors)
// motorSpeed This is the speed of the wheels of the robot based on the
// motor encoders.
// Units: degrees/second (sum of the two motors)
//
// From these state variables, the power to the motors is determined
// by this linear equation:
// power = KGYROSPEED * gyro +
// KGYROANGLE * gyroAngle +
// KPOS * motorPos +
// KSPEED * motorSpeed;
//
task taskBalance()
{
Follows(main);
float gyroSpeed, gyroAngle;
float motorSpeed;
int power, powerLeft, powerRight;
long tMotorPosOK;
long cLoop = 0;
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich beginne");
TextOut(0, LCD_LINE4, "mich nun");
TextOut(0, LCD_LINE4, "selbst zu");
TextOut(0, LCD_LINE6, "Stabilisieren!");
tMotorPosOK = CurrentTick();
// Reset the motors to make sure we start at a zero position
ResetRotationCount(LEFT_MOTOR);
ResetRotationCount(RIGHT_MOTOR);
while(true) {
CalcInterval(cLoop++);
GetGyroData(gyroSpeed, gyroAngle);
GetMotorData(motorSpeed, motorPos);
// Apply the drive control value to the motor position to get robot
// to move.
motorPos -= motorControlDrive * tInterval;
// This is the main balancing equation
power = (KGYROSPEED * gyroSpeed + // Deg/Sec from Gyro sensor
KGYROANGLE * gyroAngle) / ratioWheel + // Deg from integral of gyro
KPOS * motorPos + // From MotorRotaionCount of both motors
KDRIVE * motorControlDrive + // To improve start/stop performance
KSPEED * motorSpeed; // Motor speed in Deg/Sec
if (abs(power) < 100)
tMotorPosOK = CurrentTick();
SteerControl(power, powerLeft, powerRight);
// Apply the power values to the motors
OnFwd(LEFT_MOTOR, powerLeft);
OnFwd(RIGHT_MOTOR, powerRight);
// Check if robot has fallen by detecting that motorPos is being limited
// for an extended amount of time.
if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) {
break;
}
Wait(WAIT_TIME);
}
Off(MOTORS);
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich bin");
TextOut(0, LCD_LINE4, "hingefallen!");
TextOut(0, LCD_LINE5, "Neustart");
TextOut(0, LCD_LINE5, "erforderlich!");
}
bool Skill::Counter(const Skill& previous) const
{
if (trigger != Trigger::Defend)
return false;
return Follows(previous);
}
bool Skill::Combo(const Skill& previous) const
{
if (trigger != Trigger::Combo)
return false;
return Follows(previous);
}
// check if we are reading string we expected, return it's value (if any)
// and move the reading position
LOCAL expr_val Match(EParser p, int token)
{
#define UNEXP_CHAR(p) { \
SetError(p, ERR_UNEXPECTED_CHAR); \
return ret; \
}
char c;
expr_val ret = Get_Expr_Dummy();
switch (token)
{
case TK_DOUBLE:
// add error check here
ret.type = ET_DBL;
ret.d_val = atof(p->string + p->pos);
while (c = CURCHAR(p), c && (isdigit(c) || c == '.')) p->pos++;
Next_Token(p);
break;
case TK_INTEGER:
ret.type = ET_INT;
ret.i_val = atoi(p->string + p->pos);
while (c = CURCHAR(p), c && isdigit(c)) p->pos++;
Next_Token(p);
break;
case TK_VAR:
// add error check here
return Match_Var(p);
case TK_STR:
return Match_String(p);
case TK_AND:
if (Follows(p, "and")) { p->pos += 3; Next_Token(p); }
else if (Follows(p, "AND")) { p->pos += 3; Next_Token(p); }
else if (Follows(p, "&&")) { p->pos += 2; Next_Token(p); }
else UNEXP_CHAR(p);
break;
case TK_OR:
if (Follows(p, "or")) { p->pos += 2; Next_Token(p); }
else if (Follows(p, "OR")) { p->pos += 2; Next_Token(p); }
else if (Follows(p, "||")) { p->pos += 2; Next_Token(p); }
else UNEXP_CHAR(p);
break;
case TK_BR_O:
if (!Follows(p, "(")) UNEXP_CHAR(p);
p->pos++;
Next_Token(p);
break;
case TK_BR_C:
if (!Follows(p, ")")) UNEXP_CHAR(p);
p->pos++;
Next_Token(p);
break;
case TK_PLUS:
if (!Follows(p, "+")) UNEXP_CHAR(p);
p->pos++;
Next_Token(p);
break;
case TK_ASTERISK:
if (!Follows(p, "*")) UNEXP_CHAR(p);
p->pos++;
Next_Token(p);
break;
case TK_SLASH:
if (!Follows(p, "/")) UNEXP_CHAR(p);
p->pos++;
Next_Token(p);
break;
case TK_MINUS:
if (!Follows(p, "-")) UNEXP_CHAR(p);
p->pos++;
Next_Token(p);
break;
case TK_LT:
if (!Follows(p, "<")) UNEXP_CHAR(p);
p->pos++; Next_Token(p);
break;
case TK_LE:
if (!Follows(p, "<=")) UNEXP_CHAR(p);
p->pos += 2; Next_Token(p);
break;
case TK_EQ:
if (!Follows(p, "=")) UNEXP_CHAR(p);
p->pos++; Next_Token(p);
break;
case TK_EQ2:
if (!Follows(p, "==")) UNEXP_CHAR(p);
p->pos += 2; Next_Token(p);
break;
case TK_NE:
if (!Follows(p, "!=") && !Follows(p, "<>")) UNEXP_CHAR(p);
p->pos += 2; Next_Token(p);
break;
case TK_GE:
if (!Follows(p, ">=")) UNEXP_CHAR(p);
p->pos += 2; Next_Token(p);
break;
case TK_GT:
if (!Follows(p, ">")) UNEXP_CHAR(p);
p->pos++; Next_Token(p);
break;
case TK_ISIN:
if (!Follows(p, "isin")) UNEXP_CHAR(p);
p->pos += 4; Next_Token(p);
break;
case TK_NISIN:
if (!Follows(p, "!isin")) UNEXP_CHAR(p);
p->pos += 5; Next_Token(p);
break;
case TK_REEQ:
if (!Follows(p, "=~")) UNEXP_CHAR(p);
p->pos += 2; Next_Token(p);
break;
case TK_RENE:
if (!Follows(p, "!~")) UNEXP_CHAR(p);
p->pos += 2; Next_Token(p);
break;
}
return ret;
}
// update the lookahead based on what we see next in the input string
LOCAL void Next_Token(EParser p)
{
char c;
while(c = CURCHAR(p), c && c == ' ') p->pos++;
if (!c) { p->lookahead = TK_EOF; }
else if (Follows(p, "isin")) { p->lookahead = TK_ISIN; }
else if (Follows(p, "!isin")) { p->lookahead = TK_NISIN; }
else if (Follows(p, "mod")) { p->lookahead = TK_MOD; } // not implemented
else if (Follows(p, "and") || Follows(p, "AND") || Follows(p, "&&"))
{ p->lookahead = TK_AND; }
else if (Follows(p, "or") || Follows(p, "OR") || Follows(p, "||"))
{ p->lookahead = TK_OR; }
else if (Follows(p, "<=")) { p->lookahead = TK_LE; }
else if (Follows(p, "==")) { p->lookahead = TK_EQ2; }
else if (Follows(p, "!=")) { p->lookahead = TK_NE; }
else if (Follows(p, ">=")) { p->lookahead = TK_GE; }
else if (Follows(p, "=~")) { p->lookahead = TK_REEQ; }
else if (Follows(p, "!~")) { p->lookahead = TK_RENE; }
else if (c == '=') { p->lookahead = TK_EQ; }
else if (c == '<') { p->lookahead = TK_LT; }
else if (c == '>') { p->lookahead = TK_GT; }
else if (c == '+') { p->lookahead = TK_PLUS; }
else if (c == '*') { p->lookahead = TK_ASTERISK; }
else if (c == '-') { p->lookahead = TK_MINUS; }
else if (c == '/') { p->lookahead = TK_SLASH; }
else if (c == '(') { p->lookahead = TK_BR_O; }
else if (c == ')') { p->lookahead = TK_BR_C; }
else if ((c >= '0' && c <= '9') || c == '.') { // isdigit screamed with debug warnings here in some occasions
int dbl = 0;
const char *cp = p->string + p->pos;
while (isdigit(*cp) || *cp == '.') {
if (*cp++ == '.') dbl++;
}
if (dbl == 0) { p->lookahead = TK_INTEGER; }
else if (dbl == 1) { p->lookahead = TK_DOUBLE; }
else { SetError(p, ERR_MALFORMED_NUM); }
}
else if (c == '%') { p->lookahead = TK_VAR; }
else p->lookahead = TK_STR;
}
