task main() { while(1) { motor[port1] = 100; motor[port10] = 100; if(abs(SensorValue[I2C_1]) > 0) { //flash LED if IEM is working SensorValue[led] = true; } else if(getMotorVelocity(port1) == 0 && getMotorVelocity(port10) == 0) { SensorValue[led] = false; SensorValue[I2C_1] = 0; //if it's not or no motors connected, reset IEM readings to allow for more tests } if (time1[T1] >= 5000) { //auto-reset the green LED every 5 seconds SensorValue[led] = false; SensorValue[I2C_1] = 0; //if it's not or no motors connected, reset IEM readings to allow for more tests time1[T1] = 0; } wait1Msec(250); } }
static void motorHandleOneArg(const char *myarg_1) { const char *myarg = myarg_1; int iValue = 0; double fValue = 0; int motor_axis_no = 0; int nvals = 0; /* ADSPORT= */ if (!strncmp(myarg_1, ADSPORT_equals_str, strlen(ADSPORT_equals_str))) { int err_code; myarg_1 += strlen(ADSPORT_equals_str); err_code = motorHandleADS_ADR(myarg_1); if (err_code == -1) return; if (err_code == 0) { cmd_buf_printf("OK"); return; } RETURN_OR_DIE("%s/%s:%d myarg_1=%s err_code=%d", __FILE__, __FUNCTION__, __LINE__, myarg_1, err_code); } /* Main.*/ if (!strncmp(myarg_1, Main_dot_str, strlen(Main_dot_str))) { myarg_1 += strlen(Main_dot_str); } /* From here on, only M1. commands */ /* e.g. M1.nCommand=3 */ nvals = sscanf(myarg_1, "M%d.", &motor_axis_no); if (nvals != 1) { RETURN_OR_DIE("%s/%s:%d line=%s nvals=%d", __FILE__, __FUNCTION__, __LINE__, myarg, nvals); } AXIS_CHECK_RETURN(motor_axis_no); myarg_1 = strchr(myarg_1, '.'); if (!myarg_1) { RETURN_OR_DIE("%s/%s:%d line=%s missing '.'", __FILE__, __FUNCTION__, __LINE__, myarg); } myarg_1++; /* Jump over '.' */ if (0 == strcmp(myarg_1, "bBusy?")) { cmd_buf_printf("%d", isMotorMoving(motor_axis_no)); return; } /* bError? */ if (!strcmp(myarg_1, "bError?")) { cmd_buf_printf("%d", get_bError(motor_axis_no)); return; } /* bEnable? bEnabled? Both are the same in the simulator */ if (!strcmp(myarg_1, "bEnable?") || !strcmp(myarg_1, "bEnabled?")) { cmd_buf_printf("%d",getAmplifierOn(motor_axis_no)); return; } /* bExecute? */ if (!strcmp(myarg_1, "bExecute?")) { cmd_buf_printf("%d", cmd_Motor_cmd[motor_axis_no].bExecute); return; } /* bHomeSensor? */ if (0 == strcmp(myarg_1, "bHomeSensor?")) { cmd_buf_printf("%d", getAxisHome(motor_axis_no)); return; } /* bLimitBwd? */ if (0 == strcmp(myarg_1, "bLimitBwd?")) { cmd_buf_printf("%d", getNegLimitSwitch(motor_axis_no) ? 0 : 1); return; } /* bLimitFwd? */ if (0 == strcmp(myarg_1, "bLimitFwd?")) { cmd_buf_printf("%d", getPosLimitSwitch(motor_axis_no) ? 0 : 1); return; } /* bHomed? */ if (0 == strcmp(myarg_1, "bHomed?")) { cmd_buf_printf("%d", getAxisHomed(motor_axis_no) ? 1 : 0); return; } /* bReset? */ if (!strcmp(myarg_1, "bReset?")) { cmd_buf_printf("%d",cmd_Motor_cmd[motor_axis_no].bReset); return; } /* fAcceleration? */ if (0 == strcmp(myarg_1, "fAcceleration?")) { cmd_buf_printf("%g", cmd_Motor_cmd[motor_axis_no].acceleration); return; } /* fActPosition? */ if (0 == strcmp(myarg_1, "fActPosition?")) { cmd_buf_printf("%g", getMotorPos(motor_axis_no)); return; } /* fActVelocity? */ if (0 == strcmp(myarg_1, "fActVelocity?")) { cmd_buf_printf("%g", getMotorVelocity(motor_axis_no)); return; } /* fPosition? */ if (0 == strcmp(myarg_1, "fPosition?")) { /* The "set" value */ cmd_buf_printf("%g", cmd_Motor_cmd[motor_axis_no].position); return; } /* nCommand? */ if (0 == strcmp(myarg_1, "nCommand?")) { cmd_buf_printf("%d", cmd_Motor_cmd[motor_axis_no].command_no); return; } /* nMotionAxisID? */ if (0 == strcmp(myarg_1, "nMotionAxisID?")) { /* The NC axis id is the same as motion axis id */ printf("%s/%s:%d %s(%d)\n", __FILE__, __FUNCTION__, __LINE__, myarg_1, motor_axis_no); cmd_buf_printf("%d", motor_axis_no); return; } /* stAxisStatus? */ if (0 == strcmp(myarg_1, "stAxisStatus?")) { /* getMotorPos must be first, it calls simulateMotion() */ double fActPostion = getMotorPos(motor_axis_no); int bEnable = getAmplifierOn(motor_axis_no); int bReset = 0; int bExecute = cmd_Motor_cmd[motor_axis_no].bExecute; unsigned nCommand = 0; unsigned nCmdData = cmd_Motor_cmd[motor_axis_no].nCmdData; double fVelocity = 0; double fPosition = 0; double fAcceleration = 0; double fDecceleration = 0; int bJogFwd = 0; int bJogBwd = 0; int bLimitFwd = getPosLimitSwitch(motor_axis_no) ? 0 : 1; int bLimitBwd = getNegLimitSwitch(motor_axis_no) ? 0 : 1; double fOverride = 0; int bHomeSensor = getAxisHome(motor_axis_no); int bEnabled = bEnable; int bError = get_bError(motor_axis_no); int nErrorId = get_nErrorId(motor_axis_no); double fActVelocity = getMotorVelocity(motor_axis_no); double fActDiff = 0; int bHomed = getAxisHomed(motor_axis_no); int bBusy = isMotorMoving(motor_axis_no); cmd_buf_printf("Main.M%d.stAxisStatus=" "%d,%d,%d,%u,%u,%g,%g,%g,%g,%d," "%d,%d,%d,%g,%d,%d,%d,%u,%g,%g,%g,%d,%d", motor_axis_no, bEnable, /* 1 */ bReset, /* 2 */ bExecute, /* 3 */ nCommand, /* 4 */ nCmdData, /* 5 */ fVelocity, /* 6 */ fPosition, /* 7 */ fAcceleration, /* 8 */ fDecceleration, /* 9 */ bJogFwd, /* 10 */ bJogBwd, /* 11 */ bLimitFwd, /* 12 */ bLimitBwd, /* 13 */ fOverride, /* 14 */ bHomeSensor, /* 15 */ bEnabled, /* 16 */ bError, /* 17 */ nErrorId, /* 18 */ fActVelocity, /* 19 */ fActPostion, /* 20 */ fActDiff, /* 21 */ bHomed, /* 22 */ bBusy /* 23 */ ); return; } /* sErrorMessage? */ if (!strcmp(myarg_1, "sErrorMessage?")) { char buf[32]; /* 9 should be OK */ int nErrorId = get_nErrorId(motor_axis_no); snprintf(buf, sizeof(buf), "%x", nErrorId); cmd_buf_printf("%s", buf); return; } /* End of "get" commands, from here, set commands */ /* nCommand=3 */ nvals = sscanf(myarg_1, "nCommand=%d", &iValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].command_no = iValue; cmd_buf_printf("OK"); return; } /* nCmdData=1 */ nvals = sscanf(myarg_1, "nCmdData=%d", &iValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].nCmdData = iValue; cmd_buf_printf("OK"); return; } /* fPosition=100 */ nvals = sscanf(myarg_1, "fPosition=%lf", &fValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].position = fValue; cmd_buf_printf("OK"); return; } /* fHomePosition */ nvals = sscanf(myarg_1, "fHomePosition=%lf", &fValue); if (nvals == 1) { /* Do noting */ cmd_buf_printf("OK"); return; } /* fVelocity=20 */ nvals = sscanf(myarg_1, "fVelocity=%lf", &fValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].velocity = fValue; cmd_buf_printf("OK"); return; } /* fAcceleration=1000 */ nvals = sscanf(myarg_1, "fAcceleration=%lf", &fValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].acceleration = fValue; cmd_buf_printf("OK"); return; } /* bEnable= */ nvals = sscanf(myarg_1, "bEnable=%d", &iValue); if (nvals == 1) { setAmplifierPercent(motor_axis_no, iValue ? 100 : 0); cmd_buf_printf("OK"); return; } /* bExecute= */ nvals = sscanf(myarg_1, "bExecute=%d", &iValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].bExecute = iValue; if (!iValue) { /* bExecute=0 is always allowed, regardless the command */ motorStop(motor_axis_no); cmd_buf_printf("OK"); return; } else if (iValue == 1) { if (cmd_Motor_cmd[motor_axis_no].velocity > cmd_Motor_cmd[motor_axis_no].maximumVelocity) { fprintf(stdlog, "%s/%s:%d axis_no=%d velocity=%g maximumVelocity=%g\n", __FILE__, __FUNCTION__, __LINE__, motor_axis_no, cmd_Motor_cmd[motor_axis_no].velocity, cmd_Motor_cmd[motor_axis_no].maximumVelocity); set_nErrorId(motor_axis_no, 0x512); cmd_buf_printf("OK"); return; } switch (cmd_Motor_cmd[motor_axis_no].command_no) { case 1: { int direction = 1; if (cmd_Motor_cmd[motor_axis_no].velocity < 0) { direction = 0; cmd_Motor_cmd[motor_axis_no].velocity = -cmd_Motor_cmd[motor_axis_no].velocity; } (void)moveVelocity(motor_axis_no, direction, cmd_Motor_cmd[motor_axis_no].velocity, cmd_Motor_cmd[motor_axis_no].acceleration); cmd_buf_printf("OK"); } break; case 2: (void)movePosition(motor_axis_no, cmd_Motor_cmd[motor_axis_no].position, 1, /* int relative, */ cmd_Motor_cmd[motor_axis_no].velocity, cmd_Motor_cmd[motor_axis_no].acceleration); cmd_buf_printf("OK"); break; case 3: (void)movePosition(motor_axis_no, cmd_Motor_cmd[motor_axis_no].position, 0, /* int relative, */ cmd_Motor_cmd[motor_axis_no].velocity, cmd_Motor_cmd[motor_axis_no].acceleration); cmd_buf_printf("OK"); break; case 10: { if (cmd_Motor_cmd[motor_axis_no].homeVeloTowardsHomeSensor && cmd_Motor_cmd[motor_axis_no].homeVeloFromHomeSensor) { (void)moveHomeProc(motor_axis_no, 0, /* direction, */ cmd_Motor_cmd[motor_axis_no].nCmdData, cmd_Motor_cmd[motor_axis_no].homeVeloTowardsHomeSensor, cmd_Motor_cmd[motor_axis_no].acceleration); cmd_buf_printf("OK"); } else { cmd_buf_printf("Error : %d %g %g", 70000, cmd_Motor_cmd[motor_axis_no].homeVeloTowardsHomeSensor, cmd_Motor_cmd[motor_axis_no].homeVeloFromHomeSensor); } } break; default: RETURN_OR_DIE("%s/%s:%d line=%s command_no=%u", __FILE__, __FUNCTION__, __LINE__, myarg, cmd_Motor_cmd[motor_axis_no].command_no); } return; } RETURN_OR_DIE("%s/%s:%d line=%s invalid_iValue=%u '.'", __FILE__, __FUNCTION__, __LINE__, myarg, iValue); } /* bReset= */ nvals = sscanf(myarg_1, "bReset=%d", &iValue); if (nvals == 1) { cmd_Motor_cmd[motor_axis_no].bReset = iValue; if (iValue) { motorStop(motor_axis_no); set_nErrorId(motor_axis_no, 0); set_bError(motor_axis_no, 0); } cmd_buf_printf("OK"); return; } /* if we come here, we do not understand the command */ RETURN_OR_DIE("%s/%s:%d line=%s", __FILE__, __FUNCTION__, __LINE__, myarg); }
task usercontrol() { // User control code here, inside the loop while (true) { // This is the main execution loop for the user control program. Each time through the loop // your program should update motor + servo values based on feedback from the joysticks. // ..................................................................................... // Insert user code here. This is where you use the joystick values to update your motors, etc. // ..................................................................................... int intakeForward = vexRT[Btn5U]; int intakeBackwards = vexRT[Btn5D]; float setPoint = 98; int window = 25; int x = vexRT[Ch3]; if(abs(x) < window) x = 0; int y = vexRT[Ch4]; if(abs(y) < window) y = 0; int r = vexRT[Ch1]; int shooting = 1; int motorShoot = 0; if(abs(r) < window) r = 0; drive(x, y, r); setIntake(intakeForward*127, intakeBackwards*127); float scaleFactor = 1.0; if (vexRT[Btn7U] == 1){ setPoint = 98; scaleFactor = .90; } else if (vexRT[Btn7L] == 1){ setPoint = 91; scaleFactor = .85; } else if (vexRT[Btn7R] == 1){ setPoint = 83; scaleFactor = .80; } else if (vexRT[Btn7D] == 1){ setPoint = 78; scaleFactor = 0.75; } else if (vexRT[Btn8U] == 1){ setPoint = 73; scaleFactor = 0.70; } else if (vexRT[Btn8L] == 1){ setPoint = 68; scaleFactor = 0.65; } else if (vexRT[Btn8R] == 1){ setPoint = 61; scaleFactor = 0.60; } else if (vexRT[Btn8D] == 1){ setPoint = 54; scaleFactor = 0.55; } else { shooting = 0; } float diffRPM = setPoint - getMotorVelocity(rightShoot1); ////////////////////////////////////////////////////////// /* if (shooting && getMotorVelocity(rightShoot1) < setPoint ) { motorShoot = 100 * scaleFactor; } else if (shooting == 1 && getMotorVelocity(rightShoot1) > setPoint ) { motorShoot = 40 * scaleFactor; } else if (shooting == 1 && getMotorVelocity(rightShoot1) == setPoint) { motorShoot = 80 * scaleFactor; } */ if (shooting == 1) { motorShoot = scaleFactor*127.0; } else if(shooting == 0) { motorShoot = 0; } motor[leftShoot1] = motor[leftShoot2] = motor[rightShoot1] = motor[rightShoot2] = motorShoot; // This is the main execution loop for the user control program. Each time through the loop // your program should update motor + servo values based on feedback from the joysticks. // ..................................................................................... // Insert user code here. This is where you use the joystick values to update your motors, etc. // ..................................................................................... } }