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.
// .....................................................................................
}
}
