/* Set the PWM duty cycle of the low-side drivers. */
extern int32_t irobotPWMLowSideDrivers(
const irobotUARTPort_t port, /* (in) UART port */
uint8_t pwm0, /* (in) PWM for low-side driver 0 (0-128 duty cycle) */
uint8_t pwm1, /* (in) PWM for low-side driver 1 (0-128 duty cycle) */
uint8_t pwm2 /* (in) PWM for low-side driver 2 (0-128 duty cycle) */
){ /* (ret) Error / success code */
uint8_t packet[OP_PWM_LOW_SIDE_DRIVERS_SIZE];
pwm0 = coerce(ACTUATOR_PWM_DUTY_MIN, pwm0, ACTUATOR_PWM_DUTY_MAX);
pwm1 = coerce(ACTUATOR_PWM_DUTY_MIN, pwm1, ACTUATOR_PWM_DUTY_MAX);
pwm2 = coerce(ACTUATOR_PWM_DUTY_MIN, pwm2, ACTUATOR_PWM_DUTY_MAX);
packet[0] = OP_PWM_LOW_SIDE_DRIVERS;
packet[1] = pwm0;
packet[2] = pwm1;
packet[3] = pwm2;
return irobotUARTWriteRaw(port, packet, OP_PWM_LOW_SIDE_DRIVERS_SIZE);
}
bool coercelist(listofelements * list, element * target){
listofelements* node =list;
while(node!=NULL){
if(NULL==coerce(node->elem, target)){
here(56);
return ERROR;
}
node = node->next;
}
return OKAY;
}
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
int armTarget = 0;
int armSpeed = 0;
while (true) {
processEvents();
if (wiggle) {
if (time1[T3] % 400 >= 200) {
driveSpeedLeft=100;
driveSpeedRight=0;
} else {
driveSpeedLeft=0;
driveSpeedRight=100;
}
} else {
int maxSpd = (slowMode ? SLOWMO_DRIVE_SPEED : DRIVE_SPEED);
driveSpeedLeft = powscl(joystick.joy1_y1, maxSpd); //Drive as normal taking slow-mode into account
driveSpeedRight = powscl(joystick.joy1_y2, maxSpd);
}
motor[LeftDrive] = driveSpeedLeft; //Apply drive powers
motor[RightDrive] = driveSpeedRight;
int oldArmSpeed=armSpeed;
armSpeed = -1*powscl(
((armOwner==0) ? joystick.joy2_y2 : POVAsJoystickY(joystick.joy1_TopHat)), //R2J2-Y or R1POV-Y
ARM_SPEED);
// Position Regulator
if (armSpeed==0) {
int armPower;
if (armSpeed != oldArmSpeed) armTarget=nMotorEncoder[BlockArm]+(sgn(oldArmSpeed)*200); //Set current pos + approx error
if (armTarget < -300) { // Only correct if above 90 degrees, roughly
int error = armTarget - nMotorEncoder[BlockArm];
armPower = coerce((int)(0.1*error), -20, 20);
} else {
armPower=0;
}
nxtDisplayTextLine(3, "%i", armTarget);
motor[BlockArm] = armPower;
} else {
motor[BlockArm] = armSpeed;
}
wait1Msec(5);
}
}
static value_object div_values(value_object x, value_object y)
{
value_type common = coerce(&x, &y);
if (common == VAL_NULL)
{
fprintf(stderr, "Error: trying to divide null objects\n");
return make_null();
}
else if (common == VAL_FLOAT)
return make_float(FPL_division_64(x.float_value, y.float_value));
else if (common == VAL_NATIVE)
return make_native(x.native_value / y.native_value);
else
return make_int(x.int_value / y.int_value);
}
/* Drives in a fixed direction */
extern int32_t irobotDriveDirection(
const irobotUARTPort_t port, /* (in) UART port */
int16_t velocity, /* (in) Velocity, in mm/s */
const irobotDirection_t direction /* (in) direction */
){ /* (ret) Error / success code */
uint8_t packet[OP_DRIVE_SIZE];
velocity = coerce(ACTUATOR_WHEEL_SPEED_MIN, velocity, ACTUATOR_WHEEL_SPEED_MAX);
packet[0] = OP_DRIVE;
packet[1] = HO(velocity);
packet[2] = LO(velocity);
packet[3] = HO((uint16_t)direction);
packet[4] = LO((uint16_t)direction);
return irobotUARTWriteRaw(port, packet, OP_DRIVE_SIZE);
}
int bool_expr(boolean_t op, oprtype *addr)
{
oprtype x;
DCL_THREADGBL_ACCESS;
SETUP_THREADGBL_ACCESS;
INCREMENT_EXPR_DEPTH;
if (!eval_expr(&x))
{
DECREMENT_EXPR_DEPTH;
return FALSE;
}
assert(TRIP_REF == x.oprclass);
coerce(&x, OCT_BOOL);
bx_tail(x.oprval.tref, op, addr);
DECREMENT_EXPR_DEPTH;
return TRUE;
}
int bool_expr(bool op,oprtype *addr)
{
oprtype x;
DCL_THREADGBL_ACCESS;
SETUP_THREADGBL_ACCESS;
if (!(TREF(expr_depth))++)
TREF(expr_start) = TREF(expr_start_orig) = NULL;
if (!eval_expr(&x))
{
TREF(expr_depth) = 0;
return FALSE;
}
coerce(&x, OCT_BOOL);
if (!(--(TREF(expr_depth))))
TREF(shift_side_effects) = FALSE;
assert(x.oprclass == TRIP_REF);
bx_tail(x.oprval.tref, op, addr);
return TRUE;
}
element* copyandcoerce(element* coerced, element* target){
element* copy = copyelement(coerced);
return coerce(copy, target);
}
int indirection(oprtype *a)
{
char c;
oprtype *sb1, *sb2, subs[MAX_INDSUBSCRIPTS], x;
triple *next, *ref;
DCL_THREADGBL_ACCESS;
SETUP_THREADGBL_ACCESS;
assert(TK_ATSIGN == TREF(window_token));
if (!(TREF(expr_depth))++)
TREF(expr_start) = TREF(expr_start_orig) = NULL;
advancewindow();
if (!expratom(a))
{
TREF(expr_depth) = 0;
return FALSE;
}
coerce(a, OCT_MVAL);
ex_tail(a);
if (!(--(TREF(expr_depth))))
TREF(shift_side_effects) = FALSE;
TREF(saw_side_effect) = TREF(shift_side_effects); /* TRUE or FALSE, at this point they're the same */
if (TK_ATSIGN == TREF(window_token))
{
advancewindow();
if (TK_LPAREN != TREF(window_token))
{
stx_error(ERR_LPARENMISSING);
return FALSE;
}
ref = maketriple(OC_INDNAME);
sb1 = sb2 = subs;
for (;;)
{
if (ARRAYTOP(subs) <= sb1)
{
stx_error(ERR_MAXNRSUBSCRIPTS);
return FALSE;
}
advancewindow();
if (EXPR_FAIL == expr(sb1++, MUMPS_EXPR))
return FALSE;
if (TK_RPAREN == (c = TREF(window_token))) /* NOTE assignment */
{
advancewindow();
break;
}
if (TK_COMMA != c)
{
stx_error(ERR_RPARENMISSING);
return FALSE;
}
}
/* store argument count...n args plus the name plus the dst*/
ref->operand[0] = put_ilit((mint)(sb1 - sb2) + 2);
ins_triple(ref);
next = newtriple(OC_PARAMETER);
next->operand[0] = *a;
ref->operand[1] = put_tref(next);
*a = put_tref(ref);
while (sb2 < sb1)
{
ref = newtriple(OC_PARAMETER);
next->operand[1] = put_tref(ref);
ref->operand[0] = *sb2++;
next = ref;
}
}
return TRUE;
}
int f_incr(oprtype *a, opctype op)
{
boolean_t ok;
oprtype *increment;
triple incrchain, *oldchain, *r, *savptr, targchain, tmpexpr, *triptr;
DCL_THREADGBL_ACCESS;
SETUP_THREADGBL_ACCESS;
r = maketriple(op);
/* may need to evaluate the increment (2nd arg) early and use result later: prepare to juggle triple chains */
dqinit(&targchain, exorder); /* a place for the operation and the target */
dqinit(&tmpexpr, exorder); /* a place to juggle the shifted chain in case it's active */
triptr = TREF(expr_start);
savptr = TREF(expr_start_orig); /* but make sure expr_start_orig == expr_start since this is a new chain */
TREF(expr_start_orig) = TREF(expr_start) = &tmpexpr;
oldchain = setcurtchain(&targchain); /* save the result of the first argument 'cause it evaluates 2nd */
switch (TREF(window_token))
{
case TK_IDENT:
/* $INCREMENT() performs an implicit $GET() on a first argument lvn so we use OC_PUTINDX because
* we know only at runtime whether to signal an UNDEF error (depending on whether we have
* VIEW "NOUNDEF" or "UNDEF" state; op_putindx creates the local variable unconditionally, even if
* we have "UNDEF" state, in which case any error in op_fnincr causes an op_kill of that local variable
*/
ok = (lvn(&(r->operand[0]), OC_PUTINDX, 0));
break;
case TK_CIRCUMFLEX:
ok = gvn();
r->opcode = OC_GVINCR;
r->operand[0] = put_ilit(0); /* dummy fill since emit_code does not like empty operand[0] */
break;
case TK_ATSIGN:
ok = indirection(&r->operand[0]);
r->opcode = OC_INDINCR;
break;
default:
ok = FALSE;
break;
}
if (!ok)
{
setcurtchain(oldchain);
return FALSE;
}
TREF(expr_start) = triptr; /* restore original shift chain */
TREF(expr_start_orig) = savptr;
increment = &r->operand[1];
if (TK_COMMA != TREF(window_token))
*increment = put_ilit(1); /* default optional increment to 1 */
else
{
dqinit(&incrchain, exorder); /* a place for the increment */
setcurtchain(&incrchain); /* increment expr must evaluate before the glvn in $INCR(glvn,expr) */
advancewindow();
if (EXPR_FAIL == expr(increment, MUMPS_NUM))
{
setcurtchain(oldchain);
return FALSE;
}
dqadd(&targchain, &incrchain, exorder); /* dir before targ - this is a violation of info hiding */
setcurtchain(&targchain);
}
coerce(increment, OCT_MVAL);
ins_triple(r);
if (&tmpexpr != tmpexpr.exorder.bl)
{ /* one or more OC_GVNAME may have shifted so add to the end of the shift chain */
assert(TREF(shift_side_effects));
dqadd(TREF(expr_start), &tmpexpr, exorder); /* this is a violation of info hiding */
TREF(expr_start) = tmpexpr.exorder.bl;
assert(OC_GVSAVTARG == (TREF(expr_start))->opcode);
triptr = newtriple(OC_GVRECTARG); /* restore the result of the last gvn to preserve $referece (the naked) */
triptr->operand[0] = put_tref(TREF(expr_start));
}
if (!TREF(shift_side_effects) || (GTM_BOOL != TREF(gtm_fullbool)) || (OC_INDINCR != r->opcode))
{ /* put it on the end of the main chain as there's no reason to play more with the ordering */
setcurtchain(oldchain);
triptr = (TREF(curtchain))->exorder.bl;
dqadd(triptr, &targchain, exorder); /* this is a violation of info hiding */
} else /* need full side effects or indirect 1st argument so put everything on the shift chain */
{ /* add the chain after "expr_start" which may be much before "curtchain" */
newtriple(OC_GVSAVTARG);
setcurtchain(oldchain);
assert(NULL != TREF(expr_start));
dqadd(TREF(expr_start), &targchain, exorder); /* this is a violation of info hiding */
TREF(expr_start) = targchain.exorder.bl;
triptr = newtriple(OC_GVRECTARG);
triptr->operand[0] = put_tref(TREF(expr_start));
}
/* $increment() args need to avoid side effect processing but that's handled in expritem so eval_expr gets $i()'s SE flag */
*a = put_tref(r);
return TRUE;
}
static int coerce_color (fcolor *fg, fcolor *bg) {
int f = coerce(fg, 0.3, 0.8, .80, .1);
int b = 7 & coerce(bg, 0.3, 0.35, 1, 0);
if (f == b) f ^= 8;
return COLORING(f, b);
}
/// @copydoc scalar::coerce
/// @related proton::value
template<class T> T coerce(const value& v) { T x; coerce(v, x); return x; }
// main function
int main(void){
// initialize io
initialize_pins();
// set up sensors
ADCReadContinuously(side_ir_sensor, 0.01);
ADCReadContinuously(front_ir_sensor, 0.01);
ADCReadContinuously(goal_ir_sensor, 0.01);
// turn on status led
SetPin(green_led_pin, 1);
#ifdef WAIT_FOR_BUTTON_TO_START
// wait for button press
CallOnPinRising(react_to_button, 0, PIN_F0);
while(!running){}
#else
running=1;
#endif
// main loop
while(1){
if(running){
// poll sensors
float side_value = ADCRead(side_ir_sensor);
float front_value = ADCRead(front_ir_sensor);
float goal_value = ADCRead(goal_ir_sensor);
// sanitize values
side_value = coerce(side_value, 0, 1, side_avg[avg_idx]);
front_value = coerce(front_value, 0, 1, front_avg[avg_idx]);
goal_value = coerce(goal_value, 0, 1, goal_avg[avg_idx]);
float side_dist = 0;
float front_dist = 0;
float goal_dist = 0;
if(reset_ir_flag){
// initialize averaging arrays to values
fill_array(side_avg, AVG_LEN, side_value);
fill_array(front_avg, AVG_LEN, front_value);
fill_array(goal_avg, AVG_LEN, goal_value);
reset_ir_flag = 0;
side_dist = side_value;
front_dist = front_value;
goal_dist = goal_value;
}else{
// average values
avg_idx++;
if(avg_idx>=AVG_LEN){
avg_idx=0;
}
side_avg[avg_idx] = side_value;
front_avg[avg_idx] = front_value;
goal_avg[avg_idx] = goal_value;
side_dist = average(side_avg, AVG_LEN);
front_dist = average(front_avg, AVG_LEN);
goal_dist = average(goal_avg, AVG_LEN);
}
SetMotor(right_motor, 1);
if(GetTimeUS()/1000){
left_motor_power += 0.1;
}
SetMotor(left_motor, left_motor_power);
if(side_dist > side_to_can){
SetMotor(right_motor, 0.5);
SetMotor(left_motor, 1);
}
// follow wall
if(side_value > side_max || front_value > front_max){
SetMotor(left_motor, -1);
}else{
SetMotor(left_motor, 1);
}
SetMotor(right_motor, 1);
}
}
}
int bool_expr(boolean_t sense, oprtype *addr)
/*
* invoked to resolve expresions that are by definition coerced to Boolean, which include
* IF arguments, $SELECT() arguments, and postconditionals for both commands and arguments
* IF is the only one that comes in with the "TRUE" sense
* *addr winds up as an pointer to a jump operand, which the caller fills in
*/
{
boolean_t is_com, tv;
uint4 bexprs;
opctype c;
oprtype x;
triple *bitrip, *t, *t0, *t1, *t2;
DCL_THREADGBL_ACCESS;
SETUP_THREADGBL_ACCESS;
INCREMENT_EXPR_DEPTH;
if (!eval_expr(&x))
{
DECREMENT_EXPR_DEPTH;
return FALSE;
}
UNARY_TAIL(&x);
if (OC_LIT == (x.oprval.tref)->opcode)
{ /* if its just a literal don't waste time */
DECREMENT_EXPR_DEPTH;
return TRUE;
}
assert(TRIP_REF == x.oprclass);
coerce(&x, OCT_BOOL);
t = x.oprval.tref;
for (t1 = t; ; t1 = t2)
{
assert(TRIP_REF == t1->operand[0].oprclass);
t2 = t1->operand[0].oprval.tref;
if (!(oc_tab[t2->opcode].octype & OCT_BOOL))
break;
}
if (OC_INDGLVN == t2->opcode)
t1 = t2; /* because of how we process indirection, can't insert a NOOP between COBOOL and INDGLGN */
bitrip = maketriple(OC_BOOLINIT); /* a marker we'll delete later */
dqins(t1->exorder.bl, exorder, bitrip);
assert(TREF(curtchain) == t->exorder.fl);
(TREF(curtchain))->operand[0] = put_tref(bitrip);
bx_tail(t, sense, addr);
(TREF(curtchain))->operand[0].oprclass = NO_REF;
assert(t == x.oprval.tref);
DECREMENT_EXPR_DEPTH;
for (bexprs = 0, t0 = t; bitrip != t0; t0 = t0->exorder.bl)
{
if (OCT_JUMP & oc_tab[c = t0->opcode].octype) /* WARNING assignment */
{
switch (t0->opcode)
{
case OC_JMPFALSE:
case OC_JMPTRUE:
assert(INDR_REF == t0->operand[0].oprclass);
t0->opcode = (OC_JMPTRUE == t0->opcode) ? OC_NOOP : OC_JMP;
t0->operand[0].oprclass = (OC_NOOP == t0->opcode) ? NO_REF : INDR_REF;
if (!bexprs++)
t = t0;
break;
default:
bexprs += 2;
}
}
}
bitrip->opcode = OC_NOOP; /* ditch it after it served us */
if (1 == bexprs)
{ /* if there is just a one JMP TRUE / FALSE turn it into a literal */
assert((OC_NOOP == t->opcode) || (OC_JMP == t->opcode));
PUT_LITERAL_TRUTH((OC_NOOP == t->opcode) ^ sense, t);
t->opcode = OC_LIT;
} else if (!bexprs && (OC_COBOOL == t->opcode) && (OC_LIT == (t0 = t->operand[0].oprval.tref)->opcode)
&& ((OC_JMPEQU == t->exorder.fl->opcode) || (OC_JMPNEQ == t->exorder.fl->opcode)))
{ /* just one jump based on a literal, so resolve it */
t->opcode = OC_NOOP;
t->operand[0].oprclass = NO_REF;
t = t->exorder.fl;
dqdel(t, exorder);
unuse_literal(&t0->operand[0].oprval.mlit->v);
tv = (((0 == t0->operand[0].oprval.mlit->v.m[1]) ? OC_JMPNEQ : OC_JMPEQU) == t->opcode) ^ sense;
PUT_LITERAL_TRUTH(tv, t0);
}
return TRUE;
}
